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Topic 1 



Introduction and Motivation 



This set of lectures is about navigating mobile platforms or robots. This is a huge topic and 
in eight lectures we can only hope to undertake a brief survey. The course is an extension 
of the B4 estimation course covering topics such as linear and non-linear Kalman Filtering. 
The estimation part of the lectures is applicable to many areas of engineering not just mobile 
robotics. However I hope that couching the material in a robotics scenario will make the 
material compelling and interesting to you. 

Lets begin by dispelling some myths. For the most parts when we talk about "mobile 
robots" we are not talking about gold coloured human-shaped walking machines 1 . Instead 
we are considering some-kind of platform or vehicle that moves through its environment 
carrying some kind of payload. Almost without exception it is the payload that is of interest 
- not the platform. However the vast majority of payloads require the host vehicle to navigate 
— to be able to parameterize its position and surroundings and plan and execute trajectories 
through it. Consider some typical autonomous vehicle and payloads: 



• Humans in a airplane on autopilot (or car in the near-ish future. CMU NavLab project) 

• Scientific equipment on a Mars lander 

• Mine detectors on an autonomous underwater vehicle 

• Cargo containers in a port transport vehicle 

• Pathology media in a hospital deliver system 

1 although the Honda P5 humanoid is a good approximation 
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Verbal descriptions in a museum tour guide 

A semi-submersible drill ship (oil recovery) 

Cameras on an aerial survey drone 

Obvious military uses (tend to be single mission only...) 



All of the above require navigation. This course will hopefully give you some insight into 
how this can be achieved. 

It is worth enumerating in general terms what makes autonomous navigation so hard. The 
primary reason is that the majority of mobile robots are required to work in un-engineered 
environments. Compare the work-spaces of a welding robot in automotive plant to one that 
delivers blood samples between labs in a hospital. The former operates in a highly controlled, 
known, time invariant (apart from the thing being built) scene. If computer vision is used 
as a sensor then the workspace can be lit arbitrarily well to mitigate against shadows and 
color ambiguity. Many industrial robots work in such well known engineered environments 
that very little external sensing is needed - - they can do their job simply by controlling 
their own internal joint angles. Hospitals are a different ball-park altogether. The corridors 
are dynamic - - filling and emptying (eventually) with people on stretchers. Even if the 
robot is endowed with a map of the hospital and fitted with an upward looking camera to 
navigate off markers on the ceiling it still has to avoid fast moving obstacles (humans) while 
moving purposely towards it's goal destination. The more generic case involves coping with 
substantial scene changes (accidental or malicious ) — for example doors closing in corridors 
or furniture being moved. The thing then, that makes mobile robotics so challenging is 
uncertainty. Uncertainty is pervasive in this area and we must embrace it to make progress.... 



Topic 2 

Introduction to Path Planning and 
Obstacle Avoidance 



A basic requirement of a mobile autonomous vehicle is path planning. With the vehicle in an 
arbitrary initial position A we wish to issue a desired goal position B (including orientation) 
and have the vehicle execute a trajectory to reach B . This sounds pretty simple and we can 
think of several ways in which we could combine simple control laws that will get us from A 
to B 1 Unfortunately the waters quickly become muddied when we start talking about our 
other concerns: 



while executing its trajectory the vehicle must not smash into objects in the environ- 
ment (especially true regarding squishy humans). 

we cannot guarantee that the vehicle in question can turn-on-the-spot and would like 
to be able to operate a vehicle of arbitrary shape. These are called "kinematic" con- 
straints. 

we expect only uncertain estimates of the location of the robot and objects in the 
environment. 



The combination of path-planning, obstacle avoidance, kinematic constraints and uncer- 
tainty makes for a very hard problem indeed — one which is still an active area of research. 
However we can do some interesting things if we decouple some of the issues and make some 



for example drive in a straight line from A to B until B (x,y) is reached then rotate to align with B 
(theta). 



Holonomicity 




Figure 2.1: Two holonomic vehicles. The underwater vehicle (ODIN, University of Hawaii) 
can move in any direction irrespective of pose and the complex wheel robot PPRK (CMU) 
driven by a "Palm Pilot" uses wheels that allow slip parallel to their axis of rotation. A sum 
of translation and slip combine to achieve any motion irrespective of pose. 

simplifying assumptions. We shall begin by discussing vehicle properties and categorizing 
them into two classes — holonomic and non-holonomic. 



2.1 Holonomicity 



Holonomicity is the term used to describe the locomotive properties of a vehicle with respect 
to its workspace. We will introduce a mathematical definition of the term shortly but we 
will begin by stating, in words, a definition: 



A vehicle is holonomic if the number of local degrees of freedom of 
movement equals the number of global degrees of freedom. 



We can make this a little more clear with a few examples: 



a car is non-holonomic : the global degrees of freedom are motion in x,y and heading 
however locally, a car can only move forward or turn. It cannot slide sideways. (Even 
the turning is coupled to motion). 
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Figure 2.2: A commercial non-holonomic robot vehicle (Roombavac from iRobot coorpora- 
tion. This vehicle can be purchased for about 100 USD - but is it's utility great enough to 
warrant the prices? Discuss.... 

• The "spherical" underwater robot (Odin) and the rolling wheel vehicle in Figure 2.1 
are holonomic they can turn on the spot and translate instantaneously in any direction 
without having to rotate first. 

• A train is holonomic: it can move forwards or backwards along the track which is 
parameterised by a single global degree of freedom — the distance along the track. 

• The robot "Roombavac" (iRobot corporation) vacuum cleaner in Figure 2.1 is also 
non-holonomic. It can rotate in place but cannot slide in any direction — it needs to 
use a turn-translate-turn or turn-while-drive (like a car) paradigm to move. 

It should be obvious to you that motion control for a holonomic vehicle is much easier 
than for a non-holonomic vehicle. If this isn't obvious consider the relative complexity of 
parking a car in a tight space compared to driving a vehicle that can simply slide into the 
space sideways (a hovercraft). 

Unfortunately for us automation engineers, the vast majority of vehicles in use today 
(i.e. used by humans) are non-holonomic. In fact intrinsically holonomic vehicles are so rare 
and complex (or so simple 2 ) that we shall not discuss them further. 

We can now place some formalism on our notion of holonomicity. We say a vehicle whose 
state is parameterised by a vector x is non-holonomic if there exists a constraint Q such that 

Q(x,x,x,..-) = (2.1) 

where the state derivatives cannot be integrated out. To illustrate such a constraint we will 
take the case of a front wheel steered vehicle as shown in figure 2.1 



2 path planning for a train is quite uninteresting 
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Figure 2.3: A steered non-holonomic vehicle 

Immediately we can write a constraint expressing that the vehicle cannot slip sideways 
that is a function of x and its first derivative: 



— sin 6 

cos 6 





Q(x,x) =0 



(2.2) 



(2.3) 



The state and its derivatives are inseparable and by our definition the vehicle is non- 
holonomic. 



2.2 Configuration Space 



We are describing the robot by its state x = [#i, x 2 , • • ■ %n] ( f° r a ^D plane vehicle commonly 
X\ = x x 2 = y x% = 9 )which is a n-state parameterisation. We call the space within which 
x resides the configuration space C — space of the vehicle: 



C 



u 



(2.4) 



Vxi,X2-"Xn 



The configuration space ( or C — space ) is the set of all allowable configurations of the robot. 
For a simple vehicle moving on a plane the configuration space has the same dimension as 
the work space but for more complicated robots the dimension of the configuration space is 
much higher. Consider the case of the bomb disposal vehicle in figure 2.2. The configuration 
space for such a vehicle would be 11 — 3 for the base and another 8 for the pose of the arm 
and gripper. We can view obstacles as defining regions of C — space that are forbidden. We 
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Figure 2.4: A vehicle with a high dimensional configuration space - a commercial bomb- 
disposal platform (picture courtesy of Roboprobe Ltd. The configuration space for a human 
is immensely high dimensional — around 230. 



can label this space as C® and the remaining accessible/permissable space as C such that 
C = C® U Cq. It is often possible to define and describe the boundaries of C (and hence the 
boundaries of C in which the vehicle must move) as a constraint equation. For example if 
the workspace of a point robot in 2D x = [xy] T is bounded by a wall ax + by + c = then 



C Q = [J{x | ax + by + c> 0} 



union of all x for which ax+by+c > 



(2.5) 



If each of the constraints imposed on C — spacebj each obstacle k is represented by an 
equation of the form Ck(x) = then the open free space C admitted by n obstacles can be 
written as the following intersection: 



k=n 



c ° = n (U x i c ^ x ) = °>) 



(2.6) 



fe=i 



Equation 2.6 simple states what configurations are open to the vehicle — nothing more. Any 
path planning algorithm must guide the vehicle along a trajectory within this space while 
satisfying any non-holonomic vehicle constraints and finally deliver the vehicle to the goal 
pose. This clearly is a hard problem. Two poses that may be adjacent to each other in state 
space may require an arbitrarily long path to be executed to transition between them. Take, 
for example, the seemingly simple task of turning a vehicle through 180° when it is near 
a wall. One solution trajectory to the problem is shown in figure 2.2. The non-holonomic 
vehicle constraints conspire with the holonomic constraint imposed by the wall to require a 
complicated solution trajectory. 
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Figure 2.5: A simple task - turning through 180° quickly becomes complicated by 
C — spaceconstraints. 

2.3 The Minkowski-Sum 



Real robots have arbitrary shapes and these shapes make for complicated interactions with 
obstacles which we would like to simplify One way to do this is to transform the problem 
to one in which the robot can be considered as a point-object and a technique called the 
"Minkowski-Sum" does just this. The basic idea is to artificially inflate the extent of obstacles 
to accommodate the worst-case pose of the robot in close proximity. This is easiest to 
understand with a diagram shown in Figure 2.3. The idea is to replace each object with a 
virtual object that is the union of all poses of the vehicle that touch the obstacle. Figure 2.3 
has taken a conservative approach and "replaced" a triangular vehicle with a surrounding 
circle. The minimal Minkowski-Sum would be the union of the obstacle and all vehicle poses 
with the vehicle nose just touching it boundary. With the obstacles suitably inflated the 
vehicle can be thought of a point-object and we have a guarantee that as long as it keeps to 
the new, shrunken, free space it cannot hit an object 3 . Note it is usual to fit a polygonal 
hull around the results of the Minkowski-Sum calculation to make ensuing path planning 
calculations easier. 

So now we have a method by which we can calculate C . The next big question is how 
exactly do we plan a path through it? How do we get from an initial pose to a goal pose? 
We will consider three methods : Voronoi, "Bug" and Potential methods. 



3 This doesn't mean that awkward things won't happen — the situation shown in figure 2.2 is still 
possible. However progress can be made by planning motion such that at object boundaries the vehicle is 
always capable of moving tangentially to the boundaries 
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Figure 2.6: The Minkowski-Sum transforms a arbitrarily shaped vehicle to a point while 
inflating the obstacle. The result is guaranteed free space outside the inflated object bound- 
ary. 

2.4 Voronoi Methods 




-20 20 40 60 



00 120 140 160 



Figure 2.7: A Voronoi diagram for obstacle avoidance in the presence of point objects. 

Voronoi diagrams are elegant geometric constructions 4 that find applications throughout 
computer science -- one is shown in figure 2.4. Points on a 2D- Voronoi diagram are equi- 
distant from the nearest two objects in the real world. So the Voronoi diagram of two points 
a, b is the line bisecting them — all points on that line are equidistant from a, b. The efficient 
computation of Voronoi diagrams can be quite a complex matter but what is important 
here is that algorithms exist that when given a set of polygonal objects can generate the 
appropriate equi-distant loci. So how does this help us with path planning? Well, if we follow 
the paths defined by a Voronoi diagram we are guaranteed to stay maximally far away from 



http://www.voronoi.com/ 
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Bug Methods 15 



nearby objects. We can search the set of points on the diagram to find points that are closest 
to and visible from the start and goal positions. We initially drive to the "highway entry" 
point, follow the "Voronoi - highways" until we reach the "exit point" where we leave the 
highway and drive directly towards the goal point. 



2.5 Bug Methods 



The generation of a global Voronoi diagram requires upfront knowledge of the environment. 
In many cases this is unrealistic. Also Voronoi planners by definition keep the vehicle as far 
away from objects as possible - this can have two side effects. Firstly the robot may be using 
the objects to localise and moving away from them makes them harder to sense. Secondly 
the paths generated from a Voronoi planner can be extremely long and far from the shortest 
path (try playing with the Matlab Voronoi function). An alternative family of approaches 
go under the name of " bug algorithms" . The basic algorithm is simple: 

1. starting from A and given the coordinates of a goal pose B draw a line AB (it may 
pass through obstacles that are known or as yet unknown) 

2. move along this line until either the goal is reached or an obstacle is hit. 

3. on hitting an obstacle circumnavigate its perimeter until AB is met 

4. goto 2 

In contrast to the Voronoi approach this method keeps the vehicle as close to the obstacles 
as possible (but we won't hit them as the obstacles have been modified by the Minkowski 
sum!). However the path length could be stupidly long. A smarter modification would be 
to replace step 3 in the original algorithm with "on hitting and obstacle circumnavigate it's 
perimeter until AB is met or the line AB becomes visible in which case head for a point on 
AB closer to B " Figure 2.5 shows the kind of trajectory this modified "VisBug" algorithm 
would execute. Clearly the hugging the object boundary is not always a good plan but 
interestingly it is guaranteed to get the robot to the goal location if it is indeed reachable. 



2.6 Potential Methods 



A third very popular method of path planning is a so called "Potential Method" . Once again 
the idea is very simple. We view the goal pose as a point of low potential energy and the 
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start 



Figure 2.8: The visbug algorithm executing a goal-seek trajectory around Southern Italy. 

obstacles as areas of high potential energy. If we think of the vehicle as a ball-bearing free 
to roll around on a "potential terrain" then it will naturally roll around obstacles and down 
towards the goal point. The local curvature and magnitude of the potential field can be used 
to deduce a locally preferred motion. We now firm up this intuitive description with some 
mathematics. 

At any point x we can write the total potential Us as a sum of the potential induced U 
by k obstacles and the potential induced by the goal \J g : 



U s (x)= ^U ,(x) + U 9 (x) 



(2.7) 



i=l:k 



Now we know that the force F(x) exerted on a particle in a potential field Uj(x) can be 
written as : 



F(x) = -VU s (x) 

= -J2 VU 0ii (x) - VU 9 (x) 



(2.8) 
(2.9) 



i=l:k 



where Vis the grad operator VV = i|^ +jfr +k|p 5 This is a powerful tool - we can simply 
move the vehicle in the manner in which a particle would move in a location-dependent force- 
field. Equation 2.8 tells us the direction and magnitude of the force for any vehicle position 
x . 



5 don't get confused here with the V notation used for jacobians in the material covering non-linear 
estimation in coming pages! 
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The next question is what exactly do the potential functions look like. Well, there is 
no single answer to that -- you can "roll your own". A good choice would be to make the 
potential of an obstacle be an inverse square function of the distance between vehicle and 
obstacle. We may also choose an everywhere- convex potential for the goal so that where ever 
we start, in the absence of obstacles, we will "fall" towards the goal point. Figure 2.6 shows 
two useful potential candidates (forgive the pun). Defining p(x) as the shortest distance 



point obstacle potential 





Figure 2.9: Two typical potential functions - inverse quadratic for obstacle and quadratic 
for the goal. 

between the obstacle and the vehicle (at x ) and x s as the goal point, the algebraic functions 
for these potentials are: 




Po 



V p(x) < po 
otherwise 



(2.10) 
(2.11) 



The term po places a limit on the region of space affected by the potential field — the virtual 
vehicle is only affected when it comes with po of an obstacle. r\ is just a scale factor. Simple 
differentiation allows us to write the force vector exerted on a virtual point vehicle as: 



n 



P(x) 






1 dp(x) 
p(x) 2 cbc 



Vp(x) < po 
otherwise 



(2.12) 



where -rgp is the vector of derivatives of the distance function p(x) with respect to x,y,z. 
We proceed similarly for the goal potential. So to figure out the force acting on a virtual 
vehicle and hence the direction the real vehicle should move in, we take the sum of all obstacle 
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Potential Methods 



forces and the goal force - a very simple algorithm. The course web-site has example code 
for a potential path planner written in Matlab. Download it and play. 

Although elegant, the potential field method has one serious drawback --it only acts 
locally. There is no global path planning at play here -- the vehicle simply reacts to local 
obstacles, always moving in a direction of decreasing potential. This policy can lead to the 
vehicle getting trapped in a local minimum as shown in figure 2.6. Here the vehicle will 
descend into a local minima in front of the two features and will stop. Any other motion will 
increase its potential. If you run the code you will see the vehicle getting stuck between two 
features from time to time 6 . The potential method is someway between the bug method 
and Voronoi method in terms of distance to obstacles. The bug algorithm sticks close to 
them, the Voronoi as far away as possible. The potential method simply directs the vehicle 
in a straight line to a goal unless it moves into the vicinity of of an obstacle in which case it 
is deflected. 




Figure 2.10: A pathological potential terrain and goal combination. The vehicle can get 
stuck in a local-minima or "trap" 



6 one way out of this is to detect a trap and temporarily move the goal point, the problem is, to where? 
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Topic 3 



Estimation - A Quick Revision 



3.1 Introduction 



This lecture will begin to cover a topic central to mobile robotics - - Estimation. There 
is a vast literature on Estimation Theory encompassing a rich variation of techniques and 
ideas. We shall focus on some of the most commonly used techniques which will provide the 
tools to undertake a surprisingly diverse set of problems. We shall bring them to bear on 
real life mobile-robot problems. Although we shall discuss and illustrate these techniques 
in a domain specific fashion, you will do well to keep in your mind that these are general 
techniques and can be applied to loads of other non-robotic problems 1 . 



3.2 What is Estimation? 



To make sure we are all on the same page it is worth stating (in a wordy fashion) what we 
mean by "Estimation" : 



"Estimation is the process by which we infer the value of a quantity of interest, 
x, by processing data that is in some way dependent on x . " 



-'Tor this reason some of this material was previously taught in the B4- Estimation class 
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What is Estimation? 20 



There is nothing surprising there — it fits intuitively with our everyday meaning of the 
word. However, we may have our interest piqued when we impose the following additional 
characteristics to an estimator. 



We expect the measurements to be noise corrupted and would expect to see this un- 
certainty in input transformed to uncertainty in inference. 

We do not expect the measurements to always be of x directly. For example a GPS 
receiver processes measurements of time (4 or more satellites transmit "time-of-day- 
at-source") and yet it infers Euclidean position. 

We might like to incorporate prior information in our estimation. For example we may 
know the depth and altitude of a submarine but not its latitude. 

We might like to employ a model that tells us how we expect a system to evolve over 
time. For example given a speed and heading we could reasonably offer a model on 
how a ship might move between two time epochs — open- loop. 

We expect these models to be uncertain. Reasonably we expect this modelling error 
to be handled in the estimation process and manifest itself in the uncertainty of the 
estimated state. 



Pretty quickly we see that if we can build an estimator capable of handling the above 
conditions and requirements we are equipping ourselves with a powerful and immensely 
useful inference mechanism. 

Uncertainty is central to the problem of estimation (and robotics). After all, if it weren't 
for uncertainty many problems would have a simple algebraic solution — "give me the dis- 
tances to three known points and the algebraically-determined intersection of three circles 
will define my location". We will be trying to find answers to more realistic questions like 
"I have three measurements (all with different uncertainty) to three surveyed points (known 
roughly) - - what is your best estimate of my location?" . Clearly this is a much harder 
and more sophisticated question. But equipped with basic probability theory and a little 
calculus the next sections will derive techniques capable of answering these questions using 
a few modest assumptions. 



3.2.1 Defining the problem 

Probability theory and random- variables are the obvious way to mathematically manipulate 
and model uncertainties and as such much of this lecture will rely on your basic knowledge 
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of these subjects. We however undertake a quick review. 

We want to obtain our best estimate x for a parameter x given a set of k measurements 
Z fc = {zi,Z2 • • -Zfc}. We will use a "hat" to denote an estimated quantity and allow the 
absence of a hat to indicate the true (and unknowable) state of the variable. 

We will start by considering four illuminating estimation problems - Maximum Likeli- 
hood, Maximum a-posterior, Least Squares and Minimum Mean Squared Error. 



3.3 Maximum Likelihood Estimation 



It is sane to suppose that a measurement z that we are given is in some way related to 
the state we wish to estimate 2 . We also suppose that measurements (also referred to as 
observations in these notes) are not precise and are noise corrupted. We can encapsulate 
both the relational and uncertain aspects by defining a likelihood function: 

£ = p(z|x) (3.1) 

The distribution p(z | x) is the conditional probability of the measurement z given a particular 
value of x. Figure 3.3 is just such a distribution - - a Gaussian in this case( C is just a 
normalising constant): 

p(z|x) = i-e-K—rP-Mz-x) (32) 



Notice that equation 3.2 is a function of both x and z . Crucially we interpret C as func- 
tion of x and not z as you might initially think. Imagine we have been given an observation 
z and an associated pdf C for which we have a functional form of (Equation 3.2). We form 
our maximum likelihood estimate x m i by varying x until we find the maximum likelihood. 



Given an observation z and a likelihood function p(z|x), the maximum like- 
lihood estimator - ML finds the value of x which maximises the likelihood 
function C — p(z|x). 

x m . ( = argmaxp(zjx) (3.3) 



2 clearly, as if they were independent Z k would contain no information about x and the sensor providing 
the measurements wouldn't be much good 
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Figure 3.1: A Gaussian Likelihood function for a scalar observation z 

3.4 Maximum A-Posteriori - Estimation 



In many cases we will already have some prior knowledge on x . Imagine x is a random 
variable which we have good reason to suppose is distributed as p(x ). For example, perhaps 
we know the intended (by design) rotational velocity yU p of a CDROM drive - we might model 
our prior knowledge of this as a gaussian ~ Af(/J>p, o 2 \. If we have a sensor that can produce 
an observation z of actual rotation speed with a behaviour modelled as p(z|x) we can use 
Bayes rule to come up with a posterior pdf p(x|z) that incorporates not only the information 
from the sensor but also our assumed prior knowledge on x : 



p(x|z) 



p(z|x)p(x) 

p(z) 
C x p(z|x)p(x) 



The maximum a-posteriori - MAP finds the value of x which maximises p(z|x)p(x) (the 
normalising constant is not a function of x ). 



Given an observation z ,a likelihood function p(z|x) and a prior distribution on 
x , p(x), the maximum a posteriori estimator - MAP finds the value of x 
which maximises the posterior distribution p(x|z) 



^map 



argmaxp(z|x)p(x) 



(3.4) 



22 



Maximum A-Posteriori - Estimation 23 



Lets quickly finish the CD example, assume our sensor also has a gaussian likelihood 
function, centered on the observation z but with a variance a 2 : 



p(x) = C 1 exp{-^ r ^} 



V 

\2 



p(z|x) = C 2 exp{- - - — } 



p(z|x) 



(x - /» P ) 2 

2^ 
(z-x) 

2^ 
p(z|x)p(x) 
p(z) 
= C(z) x p(z|x) x p(x) 

^/ x r ( X ~ VpY ( Z - X )% 

=c(z)exp{ — 2^| 2^r } 

p(z|x) is maximum when the exponent is zero. A simple way to find what value of x achieves 
this is to re-write the exponent of p(z|x) as 

(x — a) 2 (x — n P ) 2 (z — x) 2 



(3 2 2a 2 2a 2 

Expanding the R.H.S and comparing coefficients swiftly leads to: 

a 2 n P + ct 2 z 
a = 2^ 2 



(3.5) 



°l + °* 



Therefore the MAP estimate x is CT;sAt 2 p , CT 2 P and it has variance 2 * p 2 . It is interesting and 
informative to note that as we increase the prior uncertainty in the CD speed by increasing 
Op towards infinity then x tends towards z — the ML estimate. In other words when we have 
an uninformative prior the MAP estimate is the same as the ML estimate. This makes sense 
as in this case the only thing providing information is the sensor (via its likelihood function). 

There several other things that you should be sure you appreciate before progressing: 

Decreasing posterior variance Calling the posterior variance o 2 ^ ( which is 1 above), 
we note it is smaller than that of the prior. In the simple example above this can 
be seen s&-^r = -m = -^2, J r-^,. This is not surprising as the measurement is adding 

information 3 



Indeed a useful metric of information is closely related to the inverse of variance — note the addition of 
inverses... 
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Update to prior In the Gaussian example above we can also write x map as an adjustment 
to the prior mean: 



V V + -T-. o X ( Z - A*p) ( 3 - 6 ) 

oi + at 



clearly if the observation matches the mode of the prior (expected) distribution then 
x map is unchanged from the prior (but it's uncertainty still decreases). Also note that 
the correction z — \x v is scaled by the relative uncertainty in both prior and observation 
- if the observation variance a\ is huge compared to a p (i.e. the sensor is pretty 
terrible) then irrespective of the magnitude of z — [i p , x map is still fi p and the decrease 
in uncertainty is small. This too makes sense -- if a sensor is very noisy we should 
not pay too much attention to disparities between expected (the prior) and measured 
values. This is a concept that we will meet again soon when we discuss Kalman 
Filtering. 



3.5 Minimum Mean Squared Error Estimation 

Another key technique for estimating the value of a random variable x is that of minimum 
mean squared error estimation. Here we assume we have been furnished with a set of 
observations Z k . We define the following cost function which we try to minimise as a 
function of x : 

x mmse = argmin£{(x - x) T (x - x)|Z k } (3.7) 

X 

The motivation is clear, we want to find an estimate of x that given all the measurement 
minimises the expected value of sum of the squared errors between the truth and estimate. 
Note also that we are trying to minimise a scalar quantity. 

Recall from probability theory that 

/oo 
g( X )p( X \y)dx (3.8) 

-oo 

the cost function is then: 

/oo 
(x-x) T (x-x)p(x|Z k )dx (3.9) 

-oo 

Differentiating the cost function and setting to zero: 

d J l X ; X ^ =2 f°° (x - x)p(x|Z k Wx = (3.10) 

»X J-oo 
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Splitting apart the integral, noting that x is a constant and rearranging: 

/oo />oo 

xp(x|Z k )(fx= / xp(x|Z k )(fx (3.11) 

-OO J — OO 

/OO /*00 

p(x|Z k )dx = / xp(x|Z k )(fx (3.12) 

-oo J —oo 



oo 

xp(x|Z k )(fx (3.13) 

■oo 

£{x|Z k } (3.14) 



Why is this important? - it tells us that the mmse estimate of a random variable 
given a whole bunch of measurements is just the mean of that variable conditioned on the 
measurements. We shall use this result time and time again in coming derivations. 

Why is it different from the Least Squares Estimator? They are related (the 
LSQ estimator can be derived from Bayes rule) but here x is a random variable where in 
the LSQ case x was a constant unknown. Note we haven't discussed the LSQ estimator yet 
- but by the time you come to revise from these notes you will have. 



3.6 Recursive Bayesian Estimation 



The idea of MAP estimation leads naturally to that of recursive Bayesian estimation. In 
MAP estimation we fused both a-priori beliefs and current measurements to come up with 
an estimate, x , of the underlying world state x . If we then took another measurement 
we could use our previous x as the prior, incorporate the new measurement and come up 
with a fresh posterior based now, on two observations. The appeal of this approach to a 
robotics application is obvious. A sensor (laser radar etc) produces a time-stamped sequence 
of observations. At each time step k we would like to obtain an estimate for it's state given 
all observations up to that time ( the set Z k ). We shall now use Bayes rule to frame this 
more precisely: 



p(x,Z k )= J5 (x|Z I >(Z k ) (3.15) 

and also 

p(x,Z k )=p(Z k |x)p(x) (3.16) 
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so 



p(x|Z k )p(Z k )=p(Z k |x)j,(x) (3-17) 

if we assume (reasonably) that given the underlying state, observations are conditionally 
independent we can write 

p(Z k |x)=p(Z fc - 1 |x)p(z fc |x) (3.18) 

where z^ is a single observation arriving at time k. Substituting into 3.17 we get 

p(x|Z k )p(Z k ) = j 5 (Z fc - 1 |x)p(z fc |x)p(x) (3.19) 

now we invoke Bayes rule to re-express the p(Z fc_1 |x) term 

t _ P(x|Z*-')p(Z*-) 

p(x) 

and substituting to reach 

Kx|Z k )p(Z k ) = p( Zk \ X ) P{xlZk ^ )P } Zk ~ 1) p( X ) (3.21) 

p(x) 

= j 5 (z fc |x) J5 (x|Z fc - 1 )p(Z fc - 1 ) (3.22) 



so 



note that 



,(x|Z k ) = ^g 1 ^ (3.23) 



Pi^Z*- 1 ) = P(Z J^? (3-24) 



p(Z k ~ l ) 

p(Z k ) 
p(Z k ~ l ) 



so 



p(Z 



fc-lN 



p(Z") p(z fc |Z*-i) 
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(3.25) 



(3.26) 
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finally then we arrive at our desired result: 



P (x|Z k ) = P(Zfc|x)p(x|Zfc " l; 



p(z k \Z^) 
where the denominator is just a normaliser 

(3.27) 

So what does this tell us? Well, we recognise p(x|Z k ) as our goal - the pdf of x conditioned 
on all observations we have received up to and including time k. The p(z*.|x) term is just the 
likelihood of the k th measurement. Finally p(x|Z fc_1 ) is a prior — it is our last best estimate 
of x at time k — 1 which at that time was conditioned on all the k — 1 measurements that had 
been made up until that time. The recursive Bayesian estimator is a powerful mechanism 
allowing new information to be added simply by multiplying a prior by a (current) likelihood. 

Note that nothing special has been said about the form of the distributions manipulated 
in the above derivation. The relationships are true whatever the form of the pdfs. However 
we can arrive at a very useful result if we consider the case where we assume Gaussian priors 
and likelihoods... the linear Kalman Filter. 
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Least Squares Estimation 



4.1 Motivation 



Imagine we have been given a vector of measurements z which we believe is related to a 
state vector of interest x by the linear equation 



z = Hx (4.1) 

We wish to take this data and solve this equation to find x in terms of z . Initially you 
may naively think that a valid solution is 

x = H- X z (4.2) 

which is only a valid solution if H is a square matrix with | H |^ — H must be invertible. 
We can get around this problem by seeking a solution x that is closest to the ideal 1 The 
metric of "closest" we choose is the following: 

x = argmin || Hx — z || 2 (4-3) 

X 

x = arg min { (Hx - z) T (Hx - z) } (4.4) 

X 

Equation 4.4 can be seen to be a "least squares" criterion. There are several ways to solve 
this problem we will describe two of them - one appealing to geometry and one to a little 
calculus. 



-"Tor this section we will assume that we have more observations than required ie dim(z) > dim(x) which 
assures that there is a unique "best" solution 

28 



Motivation 29 



4.1.1 A Geometric Solution 

Recall from basic linear algebra that the vector Hx is a linear sum of the columns of H. In 
other words Hx ranges over the column space of H. We seek a vector x such that Hx is 
closest to the data vector z . This is achieved when the error vector e = Hx — z is orthogonal 
to the space in which Hx is embedded. Thus e must be orthogonal to every column of H: 



H J (Hx - z) = (4.5) 



which can be rearranged as 



H T Hx = H T z (4.6) 

x = (H T H) _1 H T z (4.7) 

This is the least squares solution for x . The matrix (H T H) _1 H T is called the pseudo-inverse 
of H. 



4.1.2 LSQ Via Minimisation 

We can expand and set the derivative of Equation 4.4 to zero : 

1 1 Hx - z 1 1 2 = x T H T Hx - x T H T z - zHx + z T z (4.8) 

f) II Hy — 7 II 2 

011 " = 2H T Hx - 2H T z (4.9) 

ax 

= 
=*► x = (H T H) _1 H T z (4.10) 
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4.2 Weighted Least Squares 



Imagine now that we have some information regarding how reliable each of the elements in 
z is. We might express this information as a diagonal measurement covariance matrix R : 



R 



a 
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a 



:.2 



(4.12) 



It would be natural to weight each element of the error vector e according to our uncertainty 
in each element of the measurement vector z - ie by R _1 . The new minimisation becomes: 

x = argmin || R _1 (Hx — z) || 2 (4-13) 

X 

Carrying this through the same analysis yields the weighted linear least squares esti- 
mate: 

x = (H T R" 1 H)- 1 HR- 1 z (4.14) 



4.2.1 Non- linear Least Squares 

The previous section allows us to derive a least squares estimate under a linear observation 
model. However most interesting problems will involve non-linear models - measuring the 
Euclidean distance between two points for example. We now have a new minimisation task: 

|2 



x = arg min 1 1 h(x) — z 



(4.15) 



We begin by assuming we have some initial guess of a solution x . We seek a vector 5 



such that x x = x + 5 and || h(x x ) 
expansion: 



z || is minimised. To proceed we use a Taylor series 



h(x + S) = h(x ) + VH Xo 5 



h( Xl 



h(x c 



VH X0 £ 



VH X0 £-z|| 2 
yz ~ h(x )} 

b 



(4.16) 

(4.17) 
(4.18) 



where 



VH 



X(J 



dh 

<9x 



dhi 
9xi 



dhn 

cbci 



dhi 



dhn 

<9x„. 



(4.19) 



evaluated at xq 
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Equation 4.18 can be seen to be a linear least square problem - something we have already 
solved and stated in equation 4.11. By inspection then we can write an expression for S that 
minimises the right hand side of 4.18: 

5 = (VH X0 T VH X0 ) _1 VH X0 T [z - h(x )] (4.20) 

We now set x x = x +<5 and iterate again until the norm falls below a tolerance value. Like the 
linear case, there is a natural extension to the weighted non linear case. For a measurement 
z with variance R the weighted non-linear least squares algorithm is as follows: 



1. 


Begin with an initial guess x 




2. 


Evaluate 

S = (VH/R^VHs)" 1 VH/R _1 [z - 


" h(x)] 


3. 


Set x = x + 5 




4. 


If h(x) — z 2 > e goto 1 else stop. 





4.2.2 Long Baseline Navigation - an Example 

So why is the non-linear LSQ problem interesting? Well, non-linear least squares 
allows us to solve some interesting and realistic navigation problems. For example, consider 
the case of an autonomous underwater vehicle (AUV) moving within a network of acoustic 
beacons. The AUV shown in figure 4.2.2 is about to be launched on a mine detection mission 
in the Mediterranean. Within the hull (which floods) is a transceiver which emits a "call" 
pulse into the water column. Beacons deployed at known locations detect this pulse and 
reply with "acknowledge" pulses which are detected by the transceiver. The difference in 
time between "call" and "acknowledge" is proportional to the distance between vehicle and 
each beacon. Figure 4.2.2 shows a diagram of this kind of navigation (which is very similar 
to how GPS works). 

Imagine the vehicle is operating within a network of 4 beacons. We wish to find an 
LSQ estimate of the vehicle's position x^ = [x,y,z] T . Each beacon i is at known position 
Xfcj = [xu,ybi, Zbi\ T ■ We shall assume that the observations become available simultaneously 
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Figure 4.1: A Long Baseline Acoustic Network for an Autonomous Underwater Vehicle 

(AUV) 



so we can stack them into a single vector 2 . The model of the long-baseline transceiver 
operating in water with speed of sound c can be written as follows: 





z= [h 


V 


2 


u 




— 


h 


c 







u 



h U] = h(yi v 



x&i 
x fc2 
x fe3 
x 64 



x, 
x, 
x, 

X, 



(4.21) 
(4.22) 



so 



where 



VH, 



2 
re 



A,i A 
A*o A 



A x3 

A X 4 



2/1 



A 
A 



A y 3 A^ 

A y4 A y 4 



(4.23) 



£-^xi 


•£&« 


- X 


yi 


= 1/W " 


y 


A zl 


= Zbi - 


z 


r 


= \/( X bi - 



x) 2 ) + (ym - y) 2 ) + {zu - zy) 



2 In practice of course the signal that travels the furthest comes in last - the measurements are staggered. 
This is only a problem if the vehicle is moving 
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Figure 4.2: An autonomous underwater vehicle about to be launched. The large fork on the 
front is a sonar designed to detect buried mines. Within the hull is a transceiver which emits 
a "call" pulse into the water column. Beacons deployed at known locations detect this pulse 
and reply with a "acknowledge" pulses which are detected by the transceiver. The difference 
in time between "call" and "acknowledge" is proportional to the distance between vehicle 
and each beacon, (photo courtesy of MIT) see figure 4.2.2 

With the jacobian calculated and given a set of four measurements to the beacons we 
can iteratively apply the non-linear least squares algorithm until the change in x„ between 
iterations becomes small enough to disregard. You might be surprised that many of the 
oil rigs floating in the Gulf of Mexico basically use this method to calculate their position 
relative to the well-head thousands of meters below them. Oil rigs are perhaps one of the 
largest mobile-robots you are likely to encounter. 

One issue with the Least Squares solution is that enough data has to be accumulated to 
make the system observable - H T H must be invertible. For example, in our subsea example, 
acoustic-refractive properties of the water column may mean that replies from a particular 
beacon are never detected. Alternatively acoustic noise may obliterate the true signal leading 
to false detections. Figure 4.2.2 shows some real LBL data from vehicle shown in Figure 
4.2.2. Most of the noise is due to the enormously powerful fork-like sonar on its nose. 

The Kalman filter which we shall discuss, derive and use shortly is one way to overcome 
this problem of instantaneous un-observability. 
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Figure 4.3: Some real data from the ocean. The synthetic aperture adds a lot of noise to the 
LBL sensing scheme. However it is possible to separate true signal from noise as shown in 
the lower graphs. The y axes are time of flight x c giving effective distance between vehicle 
and beacons. 
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Topic 5 

Kalman Filtering -Theory, Motivation 
and Application 



Keep in mind 



The following algebra is simple but a little on the laborious side and was provided here 
for completeness. The following section will involve a linear progression of simple algebraic 
steps. I hope you will derive some degree of intellectual satisfaction in seeing the equations 
being derived using only Bayes rule. Of course, like many things in engineering, derivations 
need not be done at every invocation of a smart idea but it is important to understand the 
underlying principles. The exams will not ask for a complete derivation of the Kalman filter 
but may ask you to explain the underlying concepts with reference to the key equations. 



5.1 The Linear Kalman Filter 

We will now assume that the likelihood p(z|x) and a prior p(x) on x are Gaussian. Further- 
more, initially, we are going to model our sensor as something that produces a observation 
z which is a noise corrupted linear function of the state x : 

z = Hx + w (5.1) 

Here w is a gaussian noise with zero mean and covariance R so that 

P( - W) = (27r)"/2|R|l/2 eX P{-2 WTR " lw} - (5 ' 2) 
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Note that up until now the examples have often been dealing with scalar ID distributions. 
Here we generalise to the multidimensional case but things should still look familiar to you. 
We shall let the state have n x dimensions and the observation vector have n z dimensions. 



This allows us to write down a likelihood function: 

K z l x ) = ( 27r )W2 1 | R |i/2 ex P{"^ z " Hxf R-^z - Hx)} (5.3) 



We can also use a multidimensional Gaussian with mean x e and covariance P e to describe 
our prior belief in x : 

P( x ) = (2tt)W2 I Pe 1 1/2 ex Pi~2( x ~ x e) Tp e 1 ( x ~ x e)} (5-4) 



Now we can use Bayes rule to figure out an expression for the posterior p(x|z).: 

. . , p(z|x)p(x) 
P (x|z) = ?LUZU. (5.5) 

- P(Z|X)P(X) (5.6) 



/!L^(z|x)p(x)(ix 
_ (27r)"V 1 2|R,|i/2 ex P{-|( z - Hx) T R" 1 (z - Hx)} (2 ^ )W2 1 |Pe|1/2 exp{-|(x - x e ) T p- 1 (x - x e )} 

= W) 

(5.7) 

This looks pretty formidable but we do know that we will end up with a gaussian (gaussian 
x gaussian = gaussian) and scale factors are not important therefore we can disregard the 
denominator and focus on the product: 

exp{-^(z - HxfR-^z - Hx)} exp{-^(x - x e ) r P" 1 (x - x e )} (5.8) 

or equivalently: 

exp{-l/2 ((z - Hx) T R" 1 (z - Hx) + (x - x e )p- 1 (x - x e ) T )} (5.9) 

We know (because gaussian x gaussian = gaussian) that we can find a way to express the 
above exponent in a quadratic way: 

(x-x e ) T Pe 1 (x-x ffi ) (5.10) 
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We can figure out the new mean x e and covariance P e by expanding expression 5.9 and 
comparing terms with expression 5.10. Remember we want to find the new mean because 
Equation 3.14 tells us this will be the MMSE estimate. So expanding 5.9 we have: 



x T p- 1 x-x^p- 1 x e -x T p- 1 x e +x|p- 1 x e +x T H T R- 1 Hx-x T H T R- 1 z-zR- 1 Hx+z T R- 1 z 

(5.11) 

Now collecting terms this becomes: 

x T (p- 1 + H T R- 1 H)x-x T (p- 1 x e + H T R- 1 z)-(x r p- 1 + zR- 1 H)x+(x^p- 1 x e + z T R- 1 z) 

(5.12) 
Expanding 5.10: 

x P© x — x P e x e — x ffi P e x + x e P e x®. (5.13) 

Comparing first terms in 5.12 and 5.13 we immediately see that 

P e = (p- 1 + H T R- 1 H)- 1 . (5.14) 

Comparing the second terms we see that: 

P- I x e = p- I x e + H T R- I z. (5.15) 

Therefore we can write the MMSE estimate, x e as 

x e = (P" 1 + IP'R-^-^p-Sce + I^R^z). (5.16) 

We can combine this result with our understanding of the recursive Bayesian filter we 
covered in section 3.6. Every time a new measurement becomes available we update our 
estimate and its covariance using the above two equations. 

There is something about the above two equations 5.14 and 5.16 that may make them 
inconvenient --we have to keep inverting our prior covariance matrix which may be com- 
putationally expensive if the state-space is large 1 . Fortunately we can do some algebra to 
come up with equivalent equations that do not involve an inverse. 

We begin by stating a block matrix identity. Given matrices A , B and C the following 
is true (for non-singular A ): 

(A + BCB T )" 1 = A" 1 - A- 1 B(C" 1 + B^^WA 1 (5.17) 



Mn some navigation applications the dimension of x can approach the high hundreds 
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We can immediately apply this to 5.14 to get: 

P e = P e - P e H T (R + HP e H T )- 1 HP e (5.18) 

= P e - WSW T (5.19) 

(5.20) 



or 



(I-WH)Pe (5.21) 



where 



S = HP e H T + R (5.22) 

W = P e H T S _1 (5.23) 

Now look at the form of the update equation 5.16 it is a linear combination of x and z . 
Combining 5.20 with 5.16 we have: 

x e = (P" 1 + H T R- 1 H)- 1 (P" 1 Xe + H^^z) (5.24) 

= (P" 1 + H^^H) 1 ^^-^) + (I - WH)P e (p- 1 x e ) (5.25) 

= (P" 1 + H T R- 1 H)- 1 (H T R- 1 z) + x e + W(-Hxq) (5.26) 

= Cz + x e +W(-Hx e ) (5.27) 

where 

C = (P" 1 + H T R" 1 H)- 1 H T R- 1 (5.28) 
Taking a step aside we note that both 

H T R _1 (HP e H T + R) = H^R^HPqI^ + H T (5.29) 



and also 



so 



therefore 



(P" 1 + H T R- 1 H)P e H T = H T + I^R^HPqH 7, (5.30) 



(P" 1 + H^-^)-^^" 1 = P e H T (HP e H T + R)" 1 (5.31) 



C = PqH^- 1 (5.32) 

= Wfrom 5.23 (5.33) 
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Finally then we have 



x© = x e + W(z-Hx e ) 



(5.34) 



We can now summarise the update equations for the Linear Kalman Filter: 



The 


update equations are 


as follows. Given an 


observat 


ion z 


with 


uncertainty (covariance) R and a prior estimate x e 


with 


covariance P e the new estimate and covariance 


are calculated 


as: 


x © — 
Pe = 


x e + Wu 
P e - WSW T 






where the "Innovation" 


v is 








v = 


z — Hx e 






the 


"Innovation Covariance" S is given by 








S = 


HP e H T + R 






and the "Kalman Gain" 


W is given by 








W = 


P e H T S _1 







5.1.1 Incorporating Plant Models - Prediction 

The previous section showed how an observation vector (z ) related in some linear way to 
a state we wish to estimate (x ) can be used to update in a optimal way a prior belief 
/estimate. This analysis lead us to the so called Kalman update equations. However there 
is another case which we would like to analyse: given a prior at time k — 1, a (imperfect) 
model of a system that models the transition of state from time k — 1 to k, what is the new 
estimate at time kl 
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Mathematically we are being told that the true state behaves in the following fashion: 

x(Jfc) = Fx(fc - 1) + Bu(Jfc) + Gv(fc) (5.35) 



Here we are modelling our uncertainty in our model by adding a linear transformation 
of a "white noise" 2 term v with strength (covariance) Q. The term u(A;) is the control 
vector at time k. It models actions taken on the plant that are independent of the state 
vector itself. For example, it may model an acceleration on a model that assumes constant 
velocity. (A good example of u is the steer angle on a car input into the system by a driver 
(machine or human)) 



The i\j notation 

Our general approach will be based on an inductive argument. However first we shall intro- 
duce a little more notation. We have already shown (and used) the fact that given a set Z k 
of k observations the MMSE estimate of x is 

x mmse = £{x|Z k } (5.36) 

We now drop the "mmse" subscript and start to use two parenthesised time indexes i and 
j. We use x(i|j) to mean the "MMSE estimate of x at time i given observations up until 
time j" . Incorporating this into Equation 5.36 we have 

x(z|j) = £{x(z)|Z^'}. (5.37) 

We also use this notation to define a covariance matrix P(i\j) as 

P(i\j) = £{(x(i) - *(i|j))(x(0 - Hi\j)f\Z j } (5.38) 

which is the mean square error of the estimate x(i|j) 3 . 



2 a random variable distributed according to a zero-mean gaussian pdf 

3 If this sounds confusing focus on the fact that we are maintaining a probability distribution for x . Our 
estimate at time i using measurements up until time j (x(i|j)) is simply the mean of this distribution and 
it has variance P(i\j). If our distribution is in fact a Gaussian then these are the only statistics we need to 
fully characterize the pdf. This is exactly what a Kalman filter does — it maintains the statistics of a pdf 
that "best" represents x given a set of measurements and control inputs 
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x (i — j) is the estimate of x at time i given measurements up until time j. Com- 
monly you will see the following combinations: 

• x(A;|A;) estimate at time k given all available measurements. Often simply 
called the estimate 

• x(A;|A; — 1) estimate at time k given first k — \ measurements. This is often 
called the prediction 



Now back to our question about incorporating a plant model F and a control signal u. 
Imagine at time k we have been provided with MMSE estimate of x (k-1). Using our new 
notation we write this estimate and its covariance as x(fc — \\k — 1) and P(A; — \\k — 1). 
We are also provided with a control signal u(A;) we want to know how to figure out a new 
estimate yJyk\k — 1) at time k. Note that we are still only using measurements up until time 
(k — 1) ( i > j in Equation 5.37) however we are talking about an estimate at time k. By 
convention we call this kind of estimate a "prediction" . You can think of predicting as an 
"open loop" step as no measurements of state are used to correct the calculation. 

We can use our "conditional expectation result" to progress: 

x(jfc|fc-l) =£{x(k\Z k - 1 } (5.39) 

= £{Fx(£; - 1) + Bu(Jfe) + Gv(fc)|Z fc - 1 } (5.40) 

= F£{x(k - l)\Z k ~ 1 } + Bu(Jfe) + Ge{v{k)\L k ~ x } (5.41) 

= Fx(Jfc-l|Jfc-l) + Bu(Jfc) + (5.42) 

This is a both simple and intuitive result. It simply says that the best estimate at time k 
given measurements up until time A: — 1 is simply the projection of the last best estimate 
x(fc — l\k — 1) through the plant model. Now we turn our attention to the propagation of 
the covariance matrix through the prediction step. 



P(Jfe|Jfe - 1) = £{(x(Jfc) - x(Jfe|Jfe - l))(x(Jfe) - x(Jfe|Jfe - l)) T |Z fc " 1 } (5.43) 

perfoming the subtraction first 

x(Jfe) - x(Jfe|Jfe - 1) = (Fx(Jfe - 1) + Bu(Jfe) + Gv(fc)) - (Px(Jfe - l\k - 1) + Bu(ife)) (5.44) 
= P(x(Jfe - 1) - x(Jfe - l|Jfe - 1)) + Gv(Jfe) (5.45) 
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so 

P(jfc|jfe - 1) = £{(F(x(k - 1) - x(Jfe - l|fc - 1)) + Gv(Jfe))x (5.46) 

(F(x(fc - 1) - x(Jfc - l\k - 1)) + Gv(A;)) r |Z fc - 1 } (5.47) 

Now we are going to make a moderate assumption: that the previous estimate x(fc — l\k — 1) 
and the noise vector (modelling inaccuracies in either model or control) are uncorrelated (i.e. 
£{(x(£; — 1) — x(fc — l|fc — l))v(fc) T } is the zero matrix). This means that when we expand 
Equation 5.47 all cross terms between x(fc — l|fc — 1) and v(fc) disappear. So: 

P(Jfe|Jfe - 1) = F£{(x(fc - 1) - x(Jfe - l|fc - l))(x(Jfe - 1) - x(Jfe - l|Jfe - l)) T |Z fc - 1 }F T + 

(5.48) 

G^{v(A;)v(A;) T |Z fc - 1 }G T (5.49) 

P(Jfe|Jfe - 1) = FP(fc - l|fc - 1)F T + GQG T (5.50) 

This result should also seem familiar to you ( remember that if x ~ iV(/z, E) and y = Mx 
then y - AT(M/x,MSM T ) ?). 



5.1.2 Joining Prediction to Updates 

We are now almost in a position to put all the pieces together. To start with we insert our 
now temporal-conditional notation into the Kalman update equations. We use x.(k\k — 1) as 
the prior x e and process an observation z at time k. The posterior x e becomes x.(k\k) 



±(k\k) = x.(k\k - 1) + W(k)u(k) 
P(k\k) = P(k\k - 1) - W(k)SW(k) T 
u(k) = z(Jfe) - Hx(Jfe|Jfe - 1) 

S = HP(fc|fc-l)H T + R 
W(Jfe) =P(fc|A;-l)H T S- 1 

So now we are in a position to write down the "standard Linear Kalman Filter Equations" . 
If the previous pages of maths have started to haze your concentration wake up now as you 
will need to know and appreciate the following: 
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Linear Kalman Filter Equations 


prediction: 




x(Jfe|Jfe - 1) = Fx(A; - l\k - 1) + Bu(Jfe) 


(5.51) 


P(Jfe|Jfe - 1) = FP(A; - l\k - 1)F T + GQG T 


(5.52) 


update: 




x(Jfe|Jfe) = x(jfc|fc - 1) + W(k)u(k) 


(5.53) 


P(k\k) = P(k\k - 1) - W(k)SW(k) T 


(5.54) 


where 




u(k) = z(Jfe) - Hx(Jfe|Jfe - 1) 


(5.55) 


S = HP(A;|A;-1)H T + R 


(5.56) 


W(Jfe) =P(A;|A;-1)H T S- 1 


(5.57) 



5.1.3 Discussion 

There are several characteristics of the Kalman Filter which you should be familiar with 
and understand well. A firm grasp of these will make your task of implementing a KF for a 
robotics problem much easier. 



Recursion The Kalman Filter is recursive. The output of one iteration becomes the input 
to the next. 

Initialising Initially you will have to provide P(0|0) and x(0|0). These are initial guesses 
(hopefully derived with some good judgement) 

Structure The Kalman filter has a predictor-corrector architecture. The prediction step 
is corrected by fusion of a measurement. Note that the innovation v is a difference 
between the actual observation and the predicted observation (Hx(A;|A; — 1)). If these 
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two are identical then there is no change to the prediction in the update step — both 
observation and prediction are in perfect agreement. Fusion happens in data space. 

Asynchronisity The update step need not happen at every iteration of the filter. If at a 
given time step no observations are available then the best estimate at time k is simply 
the prediction it(k\k — 1). 

Prediction Covariance Inflation Each predict step inflates the covariance matrix — you 
can see this by the addition of GQG T . This makes sense sice we are only using a 
pre-conceived idea of how x will evolve. By our own admission - - the presence of a 
noise vector in the model — the model is inaccurate and so we would expect certainty 
to dilate (uncertainty increase) with each prediction. 

Update Covariance Deflation Each update step generally 4 deflates the covariance ma- 
trix. The subtraction of WSW T 5 during each update illustrates this. This too makes 
sense. Each observation fused contains some information and this is "added" to the 
state estimate at each update. A metric of information is related to the inverse of 
covariance -- note how in equation 5.16 H T R _1 H is added to the inverse of P, this 
might suggest to you that information is additive. Indeed, equation 5.16 is the so called 
"information form" of the Kalman Filter. 

Observability The measurement z need not fully determine the state x (ie in general H 
is not invertible). This is crucial and useful. In a least squares problem at every 
update there have to be enough measurements to solve for the state x , However the 
Kalman filter can perform updates with only partial measurements. However to get 
useful results over time the system needs to be observable otherwise the uncertainty in 
unobserved state components will grow without bound. Two factors make this possible 
: the fact that the priors presumably contain some information about the unobserved 
states (they were observed in some previous epoch) and the role of correlations. 

Correlations The covariance matrix P is highly informative. The diagonals are the princi- 
pal uncertainties in each of the state vector elements. The off diagonals tell us about 
the relationships between elements of our estimated vector x - how they are corre- 
lated. The Kalman filter implicitly uses these correlations to update states that are not 
observed directly by the measurement model! Let's take a real life example. Imagine 
we have a model [F, B Q] of a plane flying. The model will explain how the plane 
moves between epochs as a function of both state and control. For example at lOOm/s, 
nose down 10 deg, after 100ms the plane will have travelled 10m forward (y direction) 
and perhaps 1.5 m down (in z direction). Clearly the changes and uncertainties in y 
and z are correlated -- we do not expect massive changes in height for little change 



technically it never increases it 

5 which will always be positive semi definite 
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New control u(k) 
available ? 



yes 



Prediction 



x(k|k-1)= Fx(k-1|k-1)+Bu(k) 
P(k|k-1 ) = F P(k-1 |k-1 ) F T +GQG T 




New observation z(k) 
available ? 





Prepare For Update 



S = H P(k|k-1)H 
W = P(k|k-1)H T S" 1 
v(k) = z(k)-Hx(k|k-1) 



Update 



x(k|k) = x(k|k-1)+ Wv(k) 
P(k|k) = P(k|k-1)-WSW T 



Figure 5.1: Flow chart for the Kalman filter. Note the recursive structure and how simple it 
is to implement once the model matrices F and H have been specified along with the model 
uncertainty matrices R and Q . 

in distance-over-ground in normal flight conditions. Now comes the clever part. The 
plane is equipped with an altimeter radar which measures height above the ground - a 
direct measurement of z. Fusing this measurement in the Kalman filter will result in a 
change in estimated height and also a change in y-position. The reason being that the 
correlations between height and y-position maintained in the covariance matrix mean 
that changes in estimated height should imply changes in estimated y-position because 
the two states are co-dependent. The exercises associated with these lectures should 
illustrate this fact to you further. 
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5.2 Using Estimation Theory in Mobile Robotics 



Previous pages have contained a fair amount of maths taking you through the derivation of 
various estimators in a steady fashion. Remember estimation theory is at the core of many 
problems in mobile robotics — sensors are uncertain, models are always incomplete. Tools 
like the Kalman filter 6 give a powerful framework in which we can progress. 

We will now apply these freshly derived techniques to problems in mobile robotics fo- 
cussing particularly on navigation. The following sections are not just a stand-alone-example. 
Within the analysis, implementation and discussion a few new ideas are introduced which 
will be used later in the course. 



5.2.1 A Linear Navigation Problem - "Mars Lander" 



A Lander is at an altitude x above the planet and uses a time of flight radar to detect 
altitude — see Figure 5.2.1. The onboard flight controller needs estimates of both height 
and velocity to actuate control surfaces and time rocket burns. The task is to estimate both 
altitude and descent velocity using only the radar. We begin modelling by assuming that the 
vehicle has reached its terminal velocity (but this velocity may change with height slowly). 
A simple model would be: 



x(*) 



1 ST 

1 



x(Jfe 




(5.58) 



where ST is the time between epochs and the state vector x is composed of two elements 
altitude and rate of altitude change (velocity) 



x(fc) 



(5.59) 



The process noise vector is a scalar, gaussian process with covariance Q. It represents noise 
in acceleration (hence the quadratic time dependence when adding to a position-state). 

Now we need to write down the observation equations. Think of the observation model 
as "explaining the observations as a function of the thing you want to estimate" . 



D it is not the only method though 
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Figure 5.2: A simple linear navigation problem. A Lander is at an altitude x above the 
planet and uses a time of flight radar to detect altitude. The onboard flight controller needs 
estimates of both height and velocity to actuate control surfaces and time rocket burns. The 
task is to estimate both altitude and descent velocity using only the radar. 



So we can write 



z(Jfe) =Hx(fc)+w(fc) 
\h 



*(*)=[? °] 



h 



w(Jfe) 



(5.60) 
(5.61) 



These are the "truth models" of the system (note there are no "hats" or (k — k) notations 
involved). We will never known the actual value of the noises at any epoch but we model the 
imperfections in the sensor and motions models that they represent by using their covariance 
matrices (R and Q respectively) in the filter equations. 

We are now in a position to implement this example in Matlab. Section 11.1 is a print 
out of the entire source code for a solution - it can also be downloaded from the course 
website (http://www. robots. ox. ac.uk/~pnewman : teaching^,. The exact source created the 
following graphs and so all parameters can be read from the listing. It's important that 
you can reconcile all the estimation equations we have derived with the source and also that 
you understand the structure of the algorithm — the interaction of simulator, controller and 
estimator. 
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True and Estimated Trajectories 



Trajectories Error (XEst-XTrue) 
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5.2.2 Simulation Model 

The simulation model is non-linear. This of course is realistic --we may model the world 
within our filter as a well behaved linear system 7 but the physics of the real world can be 
relied upon to conspire to yield something far more complicated. However, the details of 
the models employed in the simulation are not important. One point of this example is to 
illustrate that sometimes we can get away with simple approximations to complex systems. 
We only examine the vertical descent velocity (i.e. we ignore the coriolis acceleration)— 
it's as though we were dropping a weight vertically into a layer of atmosphere. The drag 
exerted on the vehicle by the atmosphere is a function of both vehicle form-factor, velocity 
and atmospheric density as a function of height (modelled as a saturating exponential). 



'we shall shortly introduce a way to use non-linear models but the idea that the world is always more 
complicated than we care to imagine is still valid 
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Observations 




Figure 5.3: 



Vehicle Controller 



The controller implemented does two things. Firstly when the vehicle descends below a 
predetermined height it deploys a parachute which increases its effective aerodynamic drag. 
Secondly, it fires retro-burners when it drops below a second altitude threshold. A simple 
control law based on altitude and velocity is used to control the vehicle to touch down with 
a low velocity. At the point when rockets are fired, the parachute is also jettisoned. 

The important point here is that the controller operates on estimated quantities. If the 
estimated quantities are in error it is quite easy to envision bad things happening. This is a 
point common to all robotic systems — actions (involving substantial energies) are frequently 
executed on the basis estimates. The motivation to understand estimation process and its 
failure modes is clear! 



Analysis of Mars Lander Simulation 

Flight Pattern Figure 5.2.1 shows the simulated (true) and estimated states using the code 
listed in Section 11.1. Initially the vehicle is high in thin atmosphere which produces 
little drag. The vehicle accelerates through the high levels of the atmosphere. Soon the 
density increases and the vehicle brakes under the effect of drag to reach a quasi-steady 
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state terminal velocity. When the parachute opens the instantaneous increase drag 
decelerates the vehicle rapidly until another steady state terminal velocity is achieved. 
Finally shortly before landing the retro-burners are fired to achieve a smooth landing 
- essentially decelerating the vehicle smoothly until touch down. 

Fitness of Model The filter vehicle model < F,G > is one of constant velocity (noise 
in acceleration). Off-the-blocks then we should expect good estimation performance 
during periods of constant velocity. Examining the graphs we see this is true. When 
the velocity is constant both position and velocity are tracked well. However during 
periods of rapid acceleration we see poor estimates of velocity emerging. Note that 
during these times the innovation sequence and truth-estimated graphs 'spike'.... 

Derivation of Velocity Note that the observation model H does not involve the velocity 
state and yet the filter is clearly estimating and tracking velocity pretty well. At no 
point in the code can you find statements like vel = (xnew — xold) / AT . The filter 
is not explicitly differentiating position to derive velocity -- instead it is inferring it 
through the models provided. The mechanism behind this has already been discussed 
in section 5.1.3. The filter is using correlations (off diagonals) in the 2x2 matrix P 
between position and velocity states. The covariance matrix starts off being diagonal 
but during the first prediction step it becomes fully populated. 8 . Errors in position 
are correlated through the vehicle model to errors in velocity. This is easy to spot in 
the plant model as predicted position is a function of current position estimate and 
velocity estimate. Here the KF is working as an observer of a hidden state - - an 
immensely useful characteristic. However there is no free lunch. Note how during 
times of acceleration the velocity estimate lags behind the true velocity. This makes 
sense 9 the velocity state is being dragged (and hence lags) through state space by the 
correlations to directly observed position. 

Filter Tuning An obvious question to ask is how can the filter be made "tighter"? How can 
we produce a more agile tracker of velocity? The answer lies in part with the process 
noise strength Q . The addition of GQG T at each time step dilutes the interstate 
correlations. By making Q smaller we maintain stronger correlations and track inferred 
velocity. But we cannot reduce Q too far — it has to model the uncertainty in our 
model. If we reduce it too much we will have too much confidence in our predictions 
and the update stage will have little corrective effect. The process of choosing a suitable 
Q (and R ) is called tuning. It is an important part of KF deployment and can be hard 
to do in practice. Fortunately there are a few concepts that can help in this process. 
Their derivation is more suited for a course in stochastic estimation rather than mobile 



8 You should download the code and check this out for yourself. Try forcing the off diagonals to zero 
and see the effect 

"expand the KF update equations with a constant velocity model and full P matrix. Note how the 
change in velocity W(2, 1) is a function of the off diagonals 
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robotics and so some of them are stated here as a set of rules (valid for linear Gaussian 
problems); 

Innovation Sequence The innovation sequence should be a white (utterly random), zero 
mean process. It is helpful to think heuristically of the innovation as the exhaust 
from the filter. If the filter is optimal all information will have been extracted 
from previous observations and the difference between actual and predicted ob- 
servations will be noise 10 . 

S-Bound The innovation covariance matrix S provides a la 2 statistical bound on 
the innovations sequence. The ith element innovation sequence should lie with 
+/ _ \/~Su- Figure 5.2.2 shows these bounds plotted. Note how during periods 
of constant velocity the innovation sequence (a scalar sequence in this case) is 
bounded around 66 percent of the time. 

Normalised Innovation Squared The scalar quantity v(k) T S~ 1 v{k) is distributed 
according to chi-squared distribution with degree of freedom equal to the dimen- 
sion of the innovation vector u(k) 

Estimate Error In a deployed system the only information available to the engi- 
neer is the innovation sequence (see above). However if a simulation is available 
comparisons between estimated and nominal true states can be made. The filter 
should be unbiased and so the average error should be zero. 

Normalised Estimate Error Squared If we denote the error at epoch k between 
true and estimated states as x(fc) then the quantity £{£(&) P(A;|A;) _1 x(A;)} is 
distributed according to chi-squared distribution with degrees of freedom equal 
to the state dimension. 

There is some skill involved in choosing values of R and Q such that the above crite- 
ria are met, especially when the filter models are a poor representation of the truth. 
The correct thing to do here is implement a better model. If however, other engi- 
neering issues impede this course of action, the filter must be de-tuned (increase noise 
strengths) in the hope of 'lumping' un-modelled characteristics into the noise vector. 
This of course means that the filter looses any claim to optimality. 



10 There is a powerful geometric interpretation of the Kalman filter that fits closely to this analogy using 
ideas of orthogonality 
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5.3 Incorporating Non-Linear Models - The Extended 
Kalman Filter 



Up until now we have only considered linear models (although working in non-linear simu- 
lated environments). It shouldn't come as a surprise to you that the majority of real world 
applications require the use of non-linear models. Think about an everyday example - a re- 
ally simple GPS receiver sitting at x r = (x,y,z) T and measuring time of flight (equivalent) 
to a satellite sitting at x s = (x, y, z)g. Clearly the time of flight measurement is "explained" 
by the norm of the difference vector || x, r — x s . This then requires the use of a non-linear 
measurement model. Fortunately we shall see that non-linear models can easily be incorpo- 
rated into the Kalman Filter equations (yielding a filter called the Extended Kalman Filter 
or EKF) you are already familiar with. The derivations are given here for completeness and 
the results are stated in Section 5.3.3. 



5.3.1 Non- linear Prediction 



We begin by writing a general form for a non-linear plant truth model: 

x(Jfe) = f (x(Jfe - 1), u(fc), k) + v(Jfe) 
z(Jfe) = h(x(k),u(k),k) + w(k). 



(5.62) 
(5.63) 



The trick behind the EKF is to linearize the non-linear models around the "best" current 
estimate (best meaning prediction (k\k — 1) or last estimate (k — l\k — 1)). This is done 
using a Taylor series expansion. Assume we have an estimate x(fc — l\k — 1) then 



x(Jfe) = f(x(Jfc - l|Jfe - 1), u(Jfc), k) + VF x [x(fc - 1) - x(fc - l|fc - 1)] 



(5.64) 



The term VF X is understood to be the jacobian of (f ) with respect to x evaluated at an 
elsewhere specified point: 



VF, 



df_ 

<9x 



" dfi 
9xi 



_9xi 






din 
dx m 



52 



Incorporating Non-Linear Models - The Extended Kalman Filter 53 



Now we know that x(Jfe|Jfe - 1) = ^{x(A;)|Z fc - 1 } and x(Jfe - l|Jfe - 1) = £{x(k - l)|Z fc " 1 }so 

k(k\k - 1) = £{f(x(Jfc - l\k - 1), u(Jfe), k) + VF x [x(fc - 1) - x(Jfe - l|Jfe - 1)] + • • • + v(fc)|Z fc - 1 } 

(5.65) 

= £{f (x(fc - l|fc - 1), u(Jfe), fc)|Z fc_1 } + VF x [x(fc - l\k - 1) - x(fc - l|fc - 1)] 

(5.66) 

= f(x(Jfe-l|Jfe-l),u(Jfe),Jfe) (5.67) 

Which is an obvious conclusion — simply pass the last estimate through the non-linear model 
to come up with a prediction based on a control signal u (k). To figure out how to propagate 
the covariance (using only terms from the previous time step) we look at the behaviour of 
x(Jfe|Jfe - 1) = x(Jfe) - x(Jfc|Jfc - 1) because P(Jfc|fc - 1) = £{x(k\k - l)k(k\k - l) T |Z fc " 1 }. 
Understanding that the jacobians of f are evaluated at ~k(k — l\k — 1) we can write 

x(Jfe|Jfe - 1) = x(Jfe) - x(Jfe|Jfe - 1) (5.68) 

« f (x(fc -l\k- 1), u(Jfe), k) + VF x [x(fc - 1) - x(Jfe - l|fc - 1)] + v(Jfe)- (5.69) 

f(x(Jfe-l|A;-l),u(Jfe),Jfe) (5.70) 

= VF x [x(fc - 1) - x(fc - l\k - 1)] + v(Jfe) (5.71) 

= VF x [x(Jfe-l|Jfe-l)] + v(Jfe) (5.72) 



(5.73) 



So 



P(Jfc|Jfe - 1) = £{x(k\k - l)x(k\k - l) T |Z fe " 1 } (5.74) 

w ^{(VF x x(A; - l\k - 1) + v(fc))(VF x x(fc - l|Jfe - 1) + v(fc)) T |Z fc - 1 } (5.75) 

= ^{VF x x(fc - l\k - l)x(fc - l|fc - l) T VF x T |Z fe - 1 } + ^{v(A;)v(A;) T |Z fc - 1 } 

(5.76) 

= VF X P(A;-1|A;-1)VF X T + Q (5.77) 

We now have the predict equations in the case of non-linear plant models. Note that fre- 
quently the model will be in the form 

x(Jfe) = f (x(fc - 1), u(Jfe), v(Jfe), k) (5.78) 

where the noise v (k) is not simply additive. In this case one would proceed with a multivari- 
ate Taylor 11 series which swiftly becomes notationally complex and algebraically tiresome. 
However the end result is intuitive. The state prediction remains unchanged but the predic- 
tion equation becomes: 

P(jfc|jfc - 1) = VF x P(fc - l\k - 1)VF X T + VG V QVG V T (5.79) 



11 alternatively stack x and v in a new vector y and differentiate with respect to y - the same result follows. 
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where VF U is the jacobian of f w.r.t the noise vector. (Don't worry many examples to follow 
- also look at the source code provided ) It is a common practice to make the substitution 
u(A;) = u n (k) + v(k) where u n (A;) is a nominal control which is then corrupted by noise. In 
this case VG V = VG U . You will see this again soon. In the meantime, look at the example 
source code provided. 



5.3.2 Non-linear Observation Model 

Now we turn to the case of a non-linear observation model (for example a range and bearing 
sensor) of the general form 

z(Jfe) =h(x(fc))+w(fc) (5.80) 

By using the same technique used for the non-linear prediction step we can show that the 
predicted observation z(k\k — 1) (H.it(k\k — 1) in the linear case) is simply the projection of 
■k(k\k — 1) through the measurement model: 

z(k\k-l)=£{z(k)\Z k - 1 } (5.81) 

z(k\k-l) = h(x(fc|fc-l)). (5.82) 

Now we wish to derive an expression for S, the innovation covariance. We begin by expressing 
the observation and the estimated observation error z(k\k — 1) using a Taylor series: 

z(Jfe) « h(x(Jfe|Jfe - 1)) + VH x (x(£;|£; - 1) 
z(Jfe|Jfe-l) = z(k) - z(k\k - I) 

= h(k(k\k - 1)) + VH x (x(A;|A; - 1) 

= VH x (x(A;|A; - 1) - x(Jfe)) + w(Jfe) 
= VH x (x(Jfc|Jfc-l)) + w(fc) 

So the covariance of the difference between actual and predicted observations (the innovation) 
can be written as: 

S = £{z(k\k - l)z(k\k - l) T |Z fc " 1 } (5.88) 

= £{(VH x (x(£;|£; - 1)) + w(Jfe))(VH x (x(Jfe|Jfc - 1)) + w(k)) T \Z k - 1 } (5.89) 

= VH x £{x(£;|£; - l)x(Jfe|A; - l) T |Z fc - 1 }VH x T + ^{w(A;)w(A;) T |Z fc - 1 } (5.90) 

= VH x P(fc|A;-l)VH x T + R (5.91) 

You may be wondering where the £{St(k\k — l)w(fc) T |Z fc_1 } terms have gone. They evaluate 
to zero as (reasonably) we do not expect the noise in observations to be correlated to the 
error in our prediction. 

54 



*(*)) + ■■ 


■■ + w{k) 


(5.83) 
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We now have one last thing to figure out - - how does a non-linear observation model 
change the update equations resulting in it(k\k) and P(k\k)7 The procedure should now be 
becoming familiar to you: figure out an expression using a series expansion for 5c(k\k) and 
take squared conditional expectation to evaluate P(k\k). 

Assume that "somehow" we have a gain W (we'll derive this in a minute) then we can 
immediately write: 

x(Jfe|Jfe) = x(fc|fc - 1) + W ( z(Jfe) - h(x(fc|fc - 1)) (5.92) 

prediction gam observation predictedobservation 

V v ' 

innovation 

We can now use this expression to make progress in figuring out P(k\k). 

P(k\k) = £{x(£;|£;)x(£;|£;) T |Z k } (5.93) 

x(Jfe|Jfe) = x(Jfe|Jfe)-x(Jfe) (5.94) 

= x(Jfc|Jfe - 1) + W(z(fc) - h(x(fc|fc - 1))) - x(Jfe) (5.95) 

= x(Jfc|Jfc-l) + Wz(Jfe|Jfe-l)-x(Jfe) (5.96) 

and substituting equation 5.87 

= x(Jfc|Jfc - 1) + W(VH x (x(fc|A; - 1)) + w(fc)) - x(fc) (5.97) 

= x(fc|fc - 1) + WVH x x(fc|£; - 1) + Ww(Jfe) (5.98) 

= [I-WVH x ]x(Jfe|A;-l) + Ww(A;) (5.99) 

An expression for P(k\k) follows swiftly as 

P(Jfe|Jfe) = £{±(k\k - l)k(k\k - l) T |Z k } (5.100) 

= £{([1 - WVH x ]x(fc|A; - 1) + Ww(fc))([I - WVH x ]x(A;|A; - 1) + Ww(A;)) T |Z k } 

(5.101) 

= [I - WVH x ]^{x(fc|A; - l)x(Jfc|Jfe - l) T |Z k }[I - WVH X ] T + W^{w(A;)w(A;) r |Z k }W T 

(5.102) 

= [I - WVH x ]P(fc|fc - 1)[I - WVH X ] T + WRW T (5.103) 

Above, we have used the fact that the expectation of x is zero and so the cross-terms of the 
expansion evaluate to zero. We now just need to find an expression for W . Here we derive 
the gain using a little calculus. Recall that behind all of this is a quest to minimise the mean 
square estimation error (which is scalar): 

J(x) =£{5c(k\k) T 5c(k\k)\kZk} (5.104) 

= Tr(P(k\k)) (5.105) 
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Fortunately there is a closed form to the differential of such a function. If B is symmetric 
for any A then 

dTr(ABA T ) dKC T 

BA - 2AB 1^= C (5 ' 106) 

Applying this (and the chain rule) to Equation 5.103 and setting the derivative equal to zero 
we get: 

—±-t = -2[I - WVH x ]P(fc|A; - 1)VH X T + 2WR = (5.107) 

oxx 

W = P(k\k - l)VH x T (VH x P(fc|£; - 1)VH X T + R)" 1 = P(k\k - 1)VH X T S _1 

(5.108) 

where 

S = (VH x P(fc|A;-l)VH x T + R) (5.109) 



Note that substituting P(k\k — 1)VH X T = WS into Equation 5.103 leads to the form of 
the covariance update we are already familiar with: 

P(k\k) = P(k\k -1)-WSW T (5.110) 



5.3.3 The Extended Kalman Filter Equations 

We can now state in a "good to remember this box" a rule for converting the linear Kalman 
filter equations to the non-linear form: 



To convert the linear Kalman Filter to the Extended Kalman Filter simply replace 
F with VF X and H with VH X in the covariance and gain calculations only. 

The jacobians are always evaluated at the best available estimate (x.(k — l\k — 1) 
for VF X and x(Jfe|Jfe - 1) for VH X 



For completeness here are the EKF equations. (You'll need these for the class- work). If 
you don't feel you are on top of the previous maths - its not the end of the world. Make 
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sure however you are familiar with the general form of the equations, (exam useful things 
are in boxes in these notes (but not exclusively)) 



Prediction: 



plant model 



x(Jfe|Jfe - 1) = f(x(Jfe -l\k- 1), u(Jfe) , k) 

predicted state 



old state est control 

P(Jfc|fc-l) = VF x P(Jfc-l|Jfc-l)VF 

predicted covariance 



T 



old est covariance 



VG V QVG V J 

N v 

process noise 



observation model 
> -"• . 



z(Jfc|fc-l) = h(x(Jfc|fc - 1)) 

predicted obs 



Update: 



x(Jfe|Jfe) 

new state estimate 
P(fc|Jfe) 

new covariance estimate 



prediction and correction 
^ 



x(Jfe|Jfe-l) + W u(k) 



innovation 



P(Jfe|Jfe - 1) - wsw 



T 



update decreases uncertainty 



where 



measurement 



u(k)= z(Jfe) -z(k\k-l) 
W = P(A:|A;-1)VH X T S- 1 



VF, 



<9x 



8fi 

9xi 



.9xi 



9fi 

9x m 



dfn 

9x„ 







kalman gain 




S = VH x P(fc|A;-l)VH x r 


+ R 










Innovation Covariance 






rati 


9hi "I 


VH. = f = 


9xi 


9x m 


ax 


dh n 
.9x1 


dh n 

9x m . 



evaluated at x(fe — l|fc — 1) 



evaluated at x(fc|fc — 1) 



(5.111) 
(5.112) 

(5.113) 



(5.114) 
(5.115) 



(5.116) 
(5.117) 

(5.118) 
(5.119) 
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We are now in a great position, possessing some very powerful tools which we shall now 
apply to some key autonomous navigation problems. 
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Topic 6 



Vehicle Models and Odometry 



6.1 Velocity Steer Model 



This is a good point to write down a simple motion model for a mobile robotic vehicle. We 
allow the vehicle to move on a 2D surface ( a floor) and point in arbitrary directions. We 
parameterise the vehicle pose x^ (the joint of position and orientation) as 



x v 

Vv 

9„ 



(6.i; 



Figure 6.1 is a diagram of a non-holonomic (local degrees of freedom less than global 
degree of freedom) vehicle with "Ackerman" steering. The angle of the steering wheels is 
given by (f) and the instantaneous forward velocity (sometimes called throttle) is V . With 
reference to Figure 6.1, we immediately see that 



x v = VST cos(0 w ) 

y v = V5Tsin($ v ) 



(6.2) 
(6.3) 
(6.4) 



Using the instantaneous center of rotation we can calculate the rate of change of orien- 



59 



Velocity Steer Model 



60 



Instantaneous 
Center of 
Rotation 




Global Reference Frame 



Figure 6.1: A non-holonomic vehicle with Ackerman steering 



tation as a function of steer angle: 



L 

a 
a6 v = V 

e v = - 



tan(0) 
tan(0) 



(6.5) 
(6.6) 
(6.7) 



We can now discretise this model by inspection: 



x„(fc + l) = f(x t ,(fc),u(fc)); u(fc) 



X V \K T" i-j 
Vv(k+1) 

e v (k + i) 



'x v (k + 1) ■ 

Vv(k+1)- 

e v {k) 



V(k) 

m 

STV(k)cos(0 v (k)) 
STV(k)sm(9 v (k)) 

5TV(k)ta,n(d>(k)) 
L 



(6.8) 
(6.9) 



Note that we have started to lump the throttle and steer into a control vector — this makes 
sense if you think about the controlling actions of a human driver. Equation 6.9 is a model for 
a perfect, noiseless vehicle. Clearly this is a little unrealistic — we need to model uncertainty. 
One popular way to do this is to insert noise terms into the control signal u such that 

u(k) = u n (k)+v(k) (6.10) 

where u„(A;) is a nominal (intended) control signal and v(fc) is a zero mean gaussian dis- 
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tributed noise vector: 



v(fc) ~JV(0, 







u(fc) ~N(u n (k), 



cr: 



<j\ 



o~: 



(6.11) 
(6.12) 



This completes a simple probabilistic model of a vehicle. We shall now see how propa- 
gation of this model affects uncertainty in vehicle pose over time. 



6.2 Evolution of Uncertainty 



In this section we will examine how an initial uncertainty in vehicle pose increases over time 
as the vehicle moves when only the control signal u is available. Hopefully you will realise 
that one way to approach this is repetitive application of the prediction step of a Kalman 
filter discussed in Section 5.1.1. The model derived in the previous section is non-linear and 
so we will have to use the non-linear form of the prediction step. 

Assume at time k we have been given a previous best estimate of the vehicle pose x„(A; — 
l\k — 1) and an associated covariance P v (k — l\k — 1). Equations 5.67 and 5.79 have that: 



X^j I /v /v 

P„(fc|fc 



f(x(Jfc-l|fc-l),u(Jfc),fc) 

VF x P„(fc - l|fc - 1)VF X T + VF V QVF 



In this case 



Q 











Oa 



(6.13) 
(6.14) 

(6.15) 



We need to evaluate the Jacobians with respect to state and control noise at x(fc — \\k — 1) 
We do this by differentiating each row of f by each state and each control respectively: 



VF, 



VF U 





1 




-5TV sin( 
5TV cos(t 
1 



5T cos(0 v ) 
ST sin(9 v ) 

tan((/>) 
L 







8TV sec 2 Q) 



(6.16) 



(6.17) 



Figure 6.2 shows the results of iterating equations 6.13 and 6.14 using the matlab code 
printed in Section 11.2. Things are pretty much as we might expect. The uncertainty 
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uncertainty bounds for Ackermann model 









Figure 6.2: Evolution of uncertainty evolution of open loop the Ackermann Model. The 
circles are the true location of the vehicle whereas the crosses mark the dead-reckoned lo- 
cations. The orientation of the vehicle is made clear by the orientation of the triangles. 
Note the divergence between true and dead-reckoned locations. This is typical of all dead 
reckoning methods — the only thing that can be changed is the rate of divergence. 

injected into the system via the noisy control makes the estimated covariance of the vehicles 
grow without bound. 

There is an important point to make here that you must understand. In actual real life 
the real robot is integrating the noisy control signal. The true trajectory will therefore always 
drift away from the trajectory estimated by the algorithms running inside the robot. This is 
exactly the same as closing your eyes and trying to walk across University Parks. Your inner 
ears and legs give you u which you pass through your own kinematic model of your body 
in your head. Of course, one would expect a gross accumulation of error as the time spent 
walking "open loop" increases. The point is that all measurements such as velocity and 
rate of turn are measured in the vehicle frame and must be integrated, along with the noise 
on the measurements. This always leads to what is called "dead reckoning drift". Figure 
6.3 shows the effect of integrating odometry on a real robot called "B21" shown in figure 
6.3 (right). The main cause of this divergence on land vehicles is wheel slip. Typically robot 
wheels are fitted with encoders that measure the rotation of each wheel. Position is then 
an integral-function of these "wheel counts" . The problem is a wheel or radius r may have 
turned through 9 but due to slip/skid the distance travelled over the ground is only (1 — r/)r6 
where r] is an unobservable slip parameter. 
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6.3 Using Dead-Reckoned Odometry Measurements 



The model in Section 6.1 used velocity and steer angles as control input into the model. It is 
common to find that this low level knowledge is not easy to obtain or that the relationship 
between control, prior and prediction is not readily discernable. The architecture in figure 
6.3 is a typical example. 



Dead Reckoned output 
(drifts badly) 



Physics (to be estimated) 



Hidden 
Control 




Physics 



Encoder Counts 

DR Model 



Other Measurements 



Nav 



Supplied Onboard integration 

System 



Navigation (to be installed by us) 



Figure 6.3: Sometimes a navigation system will be given a dead reckoned position as input 
without recourse to the control signals that were involved. Nevertheless the dead-reckoned 
position can be converted into a control input (a stream of small motions) for use in the core 
navigation system. 

It would clearly be a bad plan to simply use a dead-reckoned odometry estimate as a 
direct measurement of state in something like a Kalman Filter. Consider Figure 6.3 which 
is the dead reckoned position of a B21 mobile robot (shown on right of figure) moving 
around some corridors. Clearly by the end of the experiment we cannot reasonably interpret 
dead-reckoned position as an unbiased measurement of position! 

The low level controller on the vehicle reads encoders on the vehicle's wheels and outputs 
an estimate (with no metric of uncertainty) of its location. We can make a guess at the 
kind of model it uses 1 . Assume it has two wheels (left and right), radius r mounted either 
side of its center of mass which in one time interval turn an amount 50 1, 50 r — as shown in 
Figure 6.3. We align a body-centered co-ordinate frame on the vehicle as shown. We want 



lr This for illustration only - the real b21 vehicle is actually a synchronous drive machine in which all four 
wheels change direction http://www.irobot.com 
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Figure 6.4: Dead Reckoned position from a B21 mobile robot. The start and end locations 
are actually the same place! See how you could roll the trajectory back over itself. This is 
typical of dead reckoned trajectories — small angular errors integrate to give massive long 
term errors 



to express the change of position of the center of the vehicle as a function of 86 1, 86 r : 

r86 r = (c - L/2)a 

r86i = (c + L/2)a 

L 86, + 86 r 



c\ 



2 86i - 86 r 
2r 



Immediately then we have 



dx 




(1 — cosa)c 


dy 


= 


csina 


dd 




—a 



Which for small a becomes: 



dx 

dy 
dd 




r(86i + 86 r )/2 

-2r(S8,-S8 r ) 
L 



(6.18) 
(6.19) 

(6.20) 
(6.21) 



(6.22) 



(6.23) 



The dead-reckoning system in the vehicle simply compounds these small changes in position 
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Figure 6.5: Geometric Construction for a two wheel drive vehicle 

and orientation to obtain a global position estimate. Starting from an initial nominal frame 
at each iteration of its sensing loop it deduces a small change in position and orientation, 
and then "adds" this to its last dead-reckoned position. Of course the "addition" is slightly 
more complex than simple adding (otherwise the x coordinate would always be zero!). What 
actually happens is that the vehicle composes successive co-ordinate transformation. This 
is an important concept (which you will probably have met in other courses but perhaps 
with a different notation) and will be discussed in the next section. 



6.3.1 Composition of Transformations 

Figure 6.3.1 shows three relationships between three coordinate frames. We can express any 
coordinate j frame with respect to another frame i as a three- vector x^j = [xy9\. Here x and 
y are translations in frame i to a point p and 9 is anti-clockwise rotation around p. We define 
two operators © and to allow us to compose (chain together) multiple transformations: 



~X-i,k 



\M 



*-i,j vt3 X.j t k 



ex, 



• ■j 



(6.24) 
(6.25) 
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Figure 6.6: Three transformations between co-ordinate frames 

With reference to figure 6.3.1 we see that x 13 = x 1)2 © x 2> 3. But what exactly are these 
operators? Well, they are just a short hand for a function of one or two transformations: 



X X © X 2 = 


X\ + X2 cos #i — y 2 sin #i~| 
j/i + x 2 sin 6*i + y 2 cos 9 1 \ 


(6.26) 




—xi cos 6*i — yi sin 6*i 






exi = 


Xi sin^! — yi cos^i 




(6.27) 



Be sure you understand exactly what these equations allow. They allow you to express 
something (perhaps a point or vehicle) described in one frame, in another alternative frame. 
We can use this notation to explain the compounding of odometry measurements. Figure 
6.3.1 shows a vehicle with a prior pose x (A; — 1). The processing of wheel rotations between 
successive readings (via Equation 6.23) has indicated a vehicle-relative transformation (i.e. 
in the frame of the vehicle) u . The task of combining this new motion u(k) with the old 
dead-reckoned estimate x to arrive at a new dead-reckoned pose x is trivial. It is simply: 



Xo(fc) =x (k- 1) 0u(fc) 



(6.28) 



We have now explained a way in which measurements of wheel rotations can be used to esti- 
mate dead-reckoned pose. However we set out to figure out a way in which a dead-reckoned 
pose could be used to form a control input or measurement into a navigation system. In 
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Global Fram< 



Figure 6.7: Using transformation compositions to compound a local odometry measurement 
with a prior dead-reckoned estimate to deduce a new dead-reckoned estimate. 

other words we are given from the low-level vehicle software a sequence x (l), x (2) • • • x (fc) 
etc and we want to figure out u (k). This is now simple — we can invert equation 6.28 to get 



u(fc) = ex (A;- 1) 0x o (/e) 



(6.29) 



Looking at the Figure 6.3.1 we can see that the transformation u(k) is equivalent to going 
back along x (fc — 1) and forward along x (fc). This gives us a small control vector u (k) 
derived from two successive dead-reckoned poses that is suitable for use in another hopefully 
less error prone navigation algorithm. Effectively equation 6.29 subtracts out the common 
dead-reckoned gross error - locally odometry is good - globally it is poor. 

We are now in a position to write down a plant model for a vehicle using a dead-reckoned 
position as a control input: 



= x v (k) e (ex (/e - 1) e x (fc)) 



v 

dr— control 



x v (k) u {k) 



(6.30) 
(6.31) 

(6.32) 



It is reasonable to ask how an initial uncertainty in vehicle pose P^ propagates over time. 
We know that one way to address this question is to propagate the second order statistics 
(covariance) of a pdf for x^ through f using equation 5.79. To do this we need to figure out the 
jacobians of equation 6.32 with respect to x„ and u. This is one area where the compositional 
representation we have adopted simplifies matters. We can define and calculate the following 
jacobians: 
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, , a 9(xi e x 2 ) 

Ji(x 1 ,x 2 )= ^ 


(6.33) 


= 


1 —%2 sin #! — y 2 cos 6\ 
1 x 2 cos 6*i — \j2 sin 6>i 
1 


(6.34) 


T , x a 5(xi e x 2 ) 

J 2 (x 1 ,x 2 )= ^ 


(6.35) 


= 


cos#! — sin^! 
sin#i cos^i 

o oi. 




(6.36) 



This allows us to write (substituting into equation 5.79) : 

V{k\k - 1) = VF x P„(fc - l\k - 1)VF X T + VF V QVF V T (6.37) 

= Ji(x„, u )P,,(A; -l\k- l)Ji(x„, u ) T + J 2 (x„, u )U J 2 (x t ,, u ) T (6.38) 

Here the matrix U describes the strength of noise in the small shifts in pose represented by 
u derived from two sequential dead-reckoned poses. A simple form of this matrix would be 
purely diagonal 2 : 



U, 








o 





2 
oy 











a 



o9. 



(6.39) 



where the diagonals are variances in odometry noise. For example if the odometry loop ran 
at 20Hz and the vehicle is moving at lm/s the magnitude of translation in u would be 5cm. 
If we say slip accounts for perhaps one percent of distance travelled we might "try" a value 
of o 2 ox = o 2 = (0.05/100) 2 . Allowing a maximum rotation of w perhaps a good starting 
guess for a 2 e would be (ty/100) 2 . These numbers will give sensible answers while the vehicle 
is moving but not when it is stopped. Even when u = the covariance P„ will continue 
to inflate. This motivates the use of a time varying U which is a function of u (k). An 
exercise you should think about 



implying we expect errors in x y and orientations to be uncorrelated which is probably not true in reality 
but we will live with this approximation for now 
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Feature Based Mapping and 
Localisation 



7.1 Introduction 



We will now apply the estimation techniques we have learnt to two very important mobile 
robotics tasks - Mapping and Localisation. Theses two tasks are fundamental to successful 
deployment of autonomous vehicles and systems for example: 



Mapping 


Managing autonomous open-cast 
mining 


GPS, Radar, Inertial 




Battlefield Surveillance 


Cameras, GPS, Inertial 




Fracture detection 


x-ray acoustic 




Sub-sea Oil field detection on an 


Acoustic Beacons, Inertial 




AUV 




Localisation 


GPS 


Satellite and time of flight 




Museum Guides 


Sonar, Scanning Laser, Cameras 




Hospital Delivery System 


Radio tags, laser cameras 



A common way to approach these problems is to parameterise both the robot pose and 
aspects of the environment's geometry into one or more state vectors. For this course we 
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will work mostly in 2D but the definitions that follow are, of course, valid for the full 3D 

case. 



7.2 Features and Maps 



We suppose that the world is populated by a set of discrete landmarks or features whose 
location / orientation and geometry (with respect to a defined coordinate frame) can be 
described by a set of parameters which we lump into a feature vector x/. 

We call a collection of n features a Map such that M = {xf ; i, Xf^, Xf^ • • • Xf >n }. To keep 
notation simple sometimes we will use M to denote the map vector which is simply the 
concatenation of all the features: 

' x f,i 



M 



Xf,2 



Xf> 



(7.1; 



In this course we will constrain ourselves to using the simplest feature possible - a point 
feature such that for the i th feature: 



Xf,j 



(7.2) 



where x and y are the coordinates of the point in a global frame of reference. Point features 
are not as uncommon as one might initially think. Points occur at the intersection of lines, 
corners of rectangles, edges of objects (regions of maximal curvature) 1 . 



7.3 Observations 



We define two distinct types of observations all denoted as z — vehicle relative and absolute. 



Absolute Absolute observations are made with the help of some external device and usually 
involve a direct measurement of some aspect of the vehicle's pose. The best examples 



^ure, our own vision system operates at a higher level recognising things like gorillas and sail-boats as 
complete entities but lower-level geometric-primitive detection is buried in there as well. 
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are a GPS and a compass. They depend on external infrastructure (taken as known) 
and are nothing to do with the map. 

Vehicle Relative This kind of observation involves sensing the relationship between the 
vehicle and its immediate surroundings -especially the map, see figure 7.3. A great 
example is the measurement of the angle and distance to a point feature with respect to 
the robot's own frame of reference. We shall also class odometry (integration of wheel 
movement) as a vehicle-relative measurement because it is not a direct measurement 
of the vehicle's state. 



Features and Landmarks 



Vehicle-Feature Relative 
Observation 




Mobile Vehicle 



Global Reference Frame 



Figure 7.1: Feature Based Navigation and Mapping 



7.4 A Probabilistic Framework 



It is informative to describe the localisation and mapping tasks in terms of likelihoods (ob- 
servation pdf) and priors. 



7.4.1 Probabilistic Localisation 



For the localisation task we assume we have been given a map and receive a sequence of 
vehicle-relative observations described by a likelihood p(Z k |M,x„). We wish to figure a pdf 



71 



A Probabilistic Framework 72 



for the vehicle pose given map and observations: 



, kx _p(Z k | M,x,)p( M,x t 
p(M,Z l 
p(Z k |M, Xt ,)p(x„|M)p(M) 



p(x„|M,Z) = „ /AT ^ (7.3) 



p(M,Z k ) 
p(Z k |M,x,)p(x,|M)p(M) 
r p(Z k |M,x)p(x,|M)p(M)dx, 



(7.4) 
(7.5) 



If we are given a nominally perfect map (i.e. told to take it as absolute truth) then p(M) = 1 
this simplifies the above: 

p(x IM Z k ) = P(Z k |M,x,Mx,|M) P (M) 



-00 

rk 



p(Z k |x„)p(x t . 



/^oP( Zk | X )p( X ^) rfx f 

p(Z k |x„)p(x<,) 



C(Z l 



(7.7) 
(7.8) 



7.4.2 Probabilistic Mapping 

For the mapping side of things we are given the vehicle location (probably derived from 
absolute observation) and an sequence of vehicle relative observations. We wish to find the 
distribution of M conditioned on Z k and x„ . 

k) = j^^pfM^ 

p(x„,z) 



= p(Z k |M,x,)p(M| Xt ,)p(x,) 

p(x„,z) 
= p(Z k |M,x t ,)p(M|x t; )p(x t ,) 
~~ /_°° oo p(Z k |M,x,)p(M|x u )p(x t ,)dM 

If we assume perfect knowledge of the vehicle location then p(x„) = 1 and so 



(7.10) 

(7.11) 
(7.12) 
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(M , k P (Z k |M,x>(M|x>(x.) 

p(M|x„,Z ) - J- (zk|M>Xw)p(M|X|>)p(Xi>)dM (7.13) 



-00 

rk 



p(Z k |M)p(M) 



fZ oP (Z^\M)p(M)dM 
p(Z k \M)p(M) 



C(Z l 



(7.14) 
(7.15) 



Equations 7.15 and 7.8 should have a familiar form --we met them when discussing 
Maximum a priori estimators, recursive bayesian estimation which collectively lead us to 
discuss and explore the Kalman filter. The Kalman filter would appear to be an excellent 
way in which to implement these equations. If we parameterise the random vectors x„ and 
M with first and second order statistics (mean and variance) then the Kalman Filter will 
calculate the MMSE estimate of the posterior. The first derivation of the Kalman filter 
presented proceeded by assuming Gaussian distributions. In this case the Kalman filter is 
the optimal Bayesian estimator. The Kalman filter provides a real time way to perform state 
estimation on board a vehicle. 



7.5 Feature Based Estimation for Mapping and Local- 
ising 



7.5.1 Feature Based Localisation 

This is the simplest task. We are given a map M containing a set of features and a stream 
of observations of measurements between the vehicle and these features (see figure 7.3). We 
assume to begin with that an oracle is telling us the associations between measurements and 
observed features. We assume the vehicle we are navigating is equipped with a range-bearing 
sensor which returns the range and bearing to point like objects (the features). We will base 
the ensuing simulation on a real vehicle - the B21 we have already met - this means that 
we will have an additional sequence of dead-reckoned positions as input into the prediction 
stage. We denote these as x 

We have already have the prediction equations from previous discussions ( Equation 
7.16): 
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x(jfe|jfe-i; 
x(jfe|jfe-i; 
p(jfe|jfe-i; 



x(fc - l\k - 1) © (6x (A; - 1) © x (&)) 
x(fc - l\k - 1) © u (fc) 



VF x P(fc - l|Jfe - 1)VF X J + VF V QVF V 



Ji(x„, u )P v (k - l\k - l)Ji(x„, u D ) + J 2 (x„, u )U J 2 (x„, u 



(7.16) 
(7.17) 
(7.18) 
(7.19) 



Now we come to the observation equation which is simply a range r, and bearing #, to 
the i th feature: 



*(*) = 



h(x(fc),w(fc)) 

a/(^ - x„(£;)) 2 + (yi - y v {k)) 2 

«^2(|3f ) - e v 

We differentiate w.r.t x^ to arrive at the observation model jacobian: 

dh 



VH X ^ 

ax 



3?i-a:„(fc) Vi-yv(k) 

r r 

Vi-yv(k) Xj-x v (k) 



(7.20) 
(7.21) 

(7.22) 

(7.23) 
(7.24) 



We assume independent errors on range and bearing measurements and use a diagonal 
observation covariance matrix R : 



R 



al 



(7.25) 



Section 11.3 is a print out of an implementation of feature based localisation using the 
models we have just discussed. You can also download the code from the course web-site 
jhttp :/ '/www. robots . ox. ac.uk/~pnewman : teaching^. Figure 7.5.1 shows the trajectory of the 
vehicle as it moves through a field of random point features. Note that the code simulates 
a sensor failure for the middle twenty percent of the mission. During this time the vehicle 
becomes more and more lost. When the sensor comes back on line there is a jump in 
estimated vehicle location back to one close to the true position (see figure 7.5.1). 



7.5.2 Feature Based Mapping 

Now we will consider the dual of Localisation - Mapping. In this case the vehicle knows 
where it is but not what is in the environment. Perhaps the vehicle is fitted with a GPS 
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Figure 7.2: Feature Based Localisation 
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Figure 7.3: Feature Based Localisation innovation and error/covariance plots. Notice how 
when no measurements are made the vehicle uncertainty grows rapidly but reduces when 
features are re-observed. The covariance bounds are 3-sigma and 1-sigma-bound on state 
error and innovation plots respectively. 

or some other localisation system using an a-priori map. To avoid initial confusion we'll 
imagine the vehicle has a 'super-gps' on board telling it where it is at all times. 



The state vector for this problem is now much larger -it will be the Map itself and 
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the concatenation of all point features. The observation equation is the same as for the 
localisation case only now the feature co-ordinates are the free variables. 

The prediction model for the state is trivial. We assume that features don't move and 

SO X(fc + l\K) map = ~X.[K\K) ma p. 




decreasing feature uncertainty 



Figure 7.4: Evolution of a map over time. The vertical axis is time (k). Covariance ellipses 
for each feature are plotted in the z = k planes. Note how the filter converges to a perfect 
map because no process noise is added in the prediction step 

Initially the map is empty and so we need some method to add "newly discovered' features 
to it. To this end we introduce a feature initialisation function Y that take as arguments 
the old state vector and an observation to a landmark and returns a new, longer state vector 
with the new feature at its end. For the case of a range bearing measurement we would have 
the following: 



x(fc|Jfc)* = y(x(k\k),z(k),x v (k\k)) 

x(fc|Jfc) 

g(x(k),z(k),x v (k\k)) 

x(fc|Jfe) 

x v + r cos((9 + 9 V ) 
y v + rsin(6> + 9 V ) 



(7.26) 

(7.27) 

(7.28) 
(7.29) 
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where the coordinates of the new feature are given by the function g: 



x/ new = g(x.(k),z(k),x. v (k\k)) 

x v + r cos(# + 6 V ) 
y v + r sin(6» + 6 V ) 



( 



(7.30) 
(7.31) 



We also need to figure out how to transform the covariance matrix P when adding a new 
feature. Of course we can use the jacobian of the transformation: 



P(Jfe|Jfc)* = VY, 



where 



VY„ 



P(fc|fc) 0' 
R 



l-rixn u nx2 

VG X VG Z 



VY, 



T 



(7.32) 



(7.33) 



Now for the mapping case VG X = as the new feature is not dependent on any element 
in the state vector (which only contains features). Therefore: 



p(k\ky 



P(k\k) 




VG Z RVG Z 



T 



(7.34) 



One thing to realise is that the observation Jacobian is now "long and thin". When 
observing feature i it is only non-zero at the "location" (indexes) of the feature in the state 
vector: 



VH, 



•0 



VH 



Xfl 







other features observedfeature other features 



(7.35) 



Figure 7.5.2 shows the evolution of the map over time. Note that as no process noise is 
ever added to the system the uncertainty in feature locations after initialisation is always 
decreasing. In the limit the map will be known perfectly. The code used to generate this 
simulation for feature based mapping can be downloaded from the course web-site. You 
might think it odd that in the limit the map becomes perfectly known. You might think 
that intuitively there should always be some residual uncertainty. The point is that the 
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vehicle is continually being told where (via the super-GPS) it is and is not changing its 
position estimate as a function of landmark observations. As a consequence all features 
are independent (check this by examining the P matrix -it is block diagonal) and each 
observation of them simply adds information and therefore reduces their uncertainty - again 
and again and again.... This is in contrast to the next section where landmark observations 
will be allowed to adjust map and vehicle estimates simultaneously! 



7.6 Simultaneous Localisation and Mapping - SLAM 



SLAM is the generalised navigation problem. It asks if it is possible for a robot, starting 
with no prior information, to move through its environment and build a consistent map of 
the entire environment. Additionally the vehicle must be able to use the map to navigate 
(localise) and hence plan and control its trajectory during the mapping process. The appli- 
cations of a robot capable of navigating , with no prior map, are diverse indeed. Domains 
in which 'man in the loop' systems are impractical or difficult such as Martian exploration, 
sub-sea surveys, and disaster zones are obvious candidates. Beyond these, the sheer increase 
in autonomy that would result from reliable, robust navigation in large dynamic environ- 
ments is simply enormous. Autonomous navigation has been an active area of research for 
many years. 

In SLAM no use is made of prior maps or external infrastructure such as GPS. A SLAM 
algorithm builds a consistent estimate of both environment and vehicle trajectory using only 
noisy proprioceptive (e.g., inertial, odometric) and vehicle-centric (e.g., radar, camera and 
laser) sensors. Importantly, even though the relative observations are of the local environ- 
ment, they are fused to create an estimate of the complete workspace. 

The SLAM problem is of fundamental importance in the quest for autonomous mobile 
machines. It binds aspects of machine learning, uncertainty management, perception, sensing 
and control to enable a machine to discover and understand its surroundings with no prior 
knowledge or external assistance. 

This section will introduce a simple feature based approach to the SLAM problem. It 
doesn't work in real life for deployments in large areas because it involves running a Kalman 
filter to estimate the entire map and vehicle state. The update of the covariance matrix 
is therefore at best proportional to the square of the number of features. Given enough 
features the system will grind to a halt. Figure 7.6 shows some of the kind of area that can 
be mapped and localised in using this technique. 
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Figure 7.5: SLAM in a real environment. These figures are screen shots from an exper- 
iment using a B21 robot in a hallway at MIT. Having built the map the vehicle used 
it to navigate home to within a few cm of its start position. Videos can be found at 
jhUp://www. robots. ox. ac.uk/~pnewman : teaching^. 

You'll be pleased to know we have pretty much done all the work required to implement 
a full SLAM algorithm. We will still employ the oracle that tells us which feature is being 
seen with each observation. All we do now is change our state vector to include both vehicle 
and map: 



A 
X — 



X,. 
Xf,l 

:x f , n 



(7.36) 



Of course to start with n = but as new features are seen the state vector is grown 
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just as in the pure mapping case. The difference now is that the observation and feature 
initialisation jacobians have two non-zero blocks, one with respect to the vehicle and one 
with respect to the observed feature. 



VH X 

VY y7 



[VH Xv -.-VH Xfl 

-l-nxra U nX 2 

VG, VG, 



l-raxn U nX 2 



[VG Xv --0-] VG Z 



(7.37) 
(7.38) 

(7.39) 



Also note that the jacobian of the prediction models have changed in an obvious way: 



VF, 



VF U 



because the prediction model is now: 



VF Xv 







I2nx2n_ 




Jl(x„,u) 








I2nx2n_ 


J2(x„, u) 








02nx2n 



(7.40) 

(7.41) 

(7.42) 
(7.43) 



x„(fc + l) 
x f,i( fc ) 

x f ,„(A;) 



x„(&) eu(fc) 

x f ,„(fc) 



(7.44) 



Note that the features are not expected to move between time steps (pretty much what you 
would hope for when using a map!) and they are noiseless. Only the vehicle has process 
noise injected into its covariance matrix during a prediction step. 

The matlab code for an EKF, feature based SLAM algorithm can be found on the course 
web-site. You should be able to recognise it as a union of previous localisation and mapping 
examples. Figure 7.6 shows a few snap shots of the algorithm running. 
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7.6.1 The role of Correlations 



Note that the covariance matrix now has some structure to it -you can partition map P r 
and vehicle P vv blocks. 



vv 

T 

vm 



(7.45) 



Of diagonals P vm are the correlations between map and vehicle. Its pretty clear that there 
should be correlations between map and vehicle as they are so interrelated. Consider the 
following sequence of events. From the moment of initialisation the feature's location is a 
function of vehicle location and so errors in the vehicle location will also appear as errors 
in feature location. Now recall the discussion regarding correlations in section 5.1.3 - 
correlations cause adjustments in one state to ripple into adjustments in other states. This 
is of course also true in this kind of approach to the SLAM problem. Remarkably every 
observation of a feature affects the estimate of every other feature in the map. It's as though 
they are all tied up together with elastic bands - pulling at one will pull at the others in 
turn. 
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There are some further characteristics of the SLAM problem that transcend the estima- 
tion method being used. You should be able to check that they are true by running the 
example code: 

• The feature uncertainties never drop below the initial uncertainty of the vehicle. This 
makes sense, if you start with say 10m of uncertainty relative to Carfax tower mapping 
and re-observing things within the Thorn building is never going to reduce your overall 
uncertainty. The best you can hope for is to reduce all features to the lower bound - 
your initial uncertainty. Compare this to the ever decreasing uncertainty of features 
in the mapping only case. 

• The feature uncertainties never increase but the vehicle uncertainty can. The prediction 
model is the identity matrix for the map - we don't expect it to move. Furthermore the 
map has a noiseless model and so the prediction step does not inflate the covariance 
of the map. 

The SLAM problem is a very topical problem and is attracting interest from the AI, 
robotics and machine learning communities. If you want to find out more just ask.... 
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Multi-modal and other Methods 



The previous discussion on Estimation, Mapping and Localisation has focused almost exclu- 
sively on estimating uni-model pdfs. Much use has been made of the Kalman filter which 
estimates the mean and covariance of a pdf which under gaussian noise assumptions is the 
MMSE estimator. The Kalman filter is an important tool (and has great value outside of 
this course) but it is not the whole story. 

The single EKF approach we have talked about so far does not really behave well in 
large environments - the big problem is that linearisation errors (that came from the Taylor 
series approximations in deriving the EKF) compound and the linearisation starts to occur 
around points that are far from the true state. There is neither the space nor time to cover in 
detail how this can be overcome. Broadly though, one of two schemes are adopted. The first 
partitions the world into local maps and uses uni-modal methods (EKF) within each map. 
The maps are then carefully glued back together to form a unified global map. Secondly 
uni-modal methods are abandoned altogether. Instead, monte-carlo methods are used. 



8.1 Montecarlo Methods - Particle Filters 



The basic idea is simple. We maintain lots of different versions of the state vector - - all 
slightly different from each other. When a measurement comes in we score how well each 
version explains the data. We then make copies of the best fitting vectors and randomly 
perturb them (the equivalent of adding Q in the EKF) to form a new generation of candidate 
states. Collectively these thousands of possible states and their scores define the pdf we are 
seeking to estimate. We never have to make the assumption of Gaussian noise or perform 
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a linearisation. The big problem with this elegant approach is the number of sample-states 
(versions) we need to try out increase geometrically with the number of states being esti- 
mated. For example to estimate a ID vector we may keep 100 samples. For a 2D vector 
we would require something like 100 2 , for 3D 100 3 and so on. The trick often played is to 
be smart about selecting which samples should be copied between timesteps. This is a very 
current area of research. 



The PF-Localisation algorithm proceeds as follows: 



1. Initialise n particles in a map. Each particle is a 3 by 1 state vector of the vehicle 

2. Apply the plant model to each particle. In contrast to the prediction step in the 
Kalman approach we actually inject the process noise as well — i.e we add a random 
vector to the control vector u. This is in some ways analogous to adding GUG T to 
the predicted covariance in the Kalman approach because it increases the variance of 
the estimate (in this case the particle set). 

3. For each particle predict the observation. Compare this to the measured value. Use a 
likelihood function to evaluate the likelihood of the observation given the state repre- 
sented by each particle. This will be a scalar L which is associated with each particle. 
This scalar is referred to as a "weight" or "importance" . 

4. Select the particles that best explain the observation. One way to do this would be to 
simply sort and select the top candidates based on L but this would reduce the number 
of particles. One solution to this would be to copy from this "winning set" until we 
have n particles again. This has a hidden consequence though: it would artificially 
reduce the diversity (spread) of the particle set. Instead we randomly sample from the 
samples biased by the importance values. This means that there is a finite chance that 
particles that really didn't explain the observation well, will be reselected. This may 
turn out to be a good plan because they may do a better job for the next observation. 

5. Goto 2 



Figure 8.1 shows this process graphically for the localisation problem. A common ques- 
tion is "what is the estimate?" . Well technically it is the distribution of particles themselves 
which represents a pdf. If you want a single vector estimate, the mode, mean and median are 
all viable options. However you need to think carefully. If you run the code yourself initially 
you will see distinct clouds of particles representing differing modes of the pdf - the mean 
is somewhere between them all in a location with no support 1 ! As with all the algorithms 



however soon the clouds should coverge into one large cloud as the motion of the vehicle disambiguates 
its location 
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in the course you are strongly advised to down load the code (MCL.m) and peruse it to 
understand the translation from concept to implementation. In summary, the particle filter 
approach is very elegant and very simple to implement (see the code on the website). A 
crucial point is that it does not require any linearisation assumptions (there are no jacobians 
involved) and there are no Gaussian assumptions. It is particularly well suited for problems 
with small state spaces - the example code has a state dimension of three. 



8.2 Grid Based Mapping 



Up until now we have adopted a feature based approach to navigation. We assumed that the 
environment contains features possessing some geometry that can be extracted by suitable 
processing of sensor data - edges, points, lines etc. We then store and estimate (using further 
measurements) this sparse parameterisation (often by stacking all the feature parameters in 
to a long state vector and calling it x ). 

In contrast grid based approaches tend not to reason about the geometry of features in 
the environment but take a more sensor- centric view. A mesh of cells (a grid) is laid over the 
(initially unknown) world. Each cell c(i,j) has a number associated with it corresponding 
to an occupancy probability - p (i,j) and a probability of being empty p e . So for example 
if p o (40, 2) = 1 we are sure that the cell at (40,20) is occupied. We make no assertions as to 
what is occupying it just that something or part of something is at the coordinates covered 
by cell(40,20). 

We use two functions to model the behaviour of the sensor - one to express the likelihood 
of a cells within the observed region being empty and one to express the likelihood of them 
being occupied. Figure 8.2 shows one possible set of functions for a widebeam in-air sonar. 
For each cell under the beam a Bayes rule update is used to figure out the new probability 
of a cell being empty or being occupied. As the sensor (mounted on a vehicle) moves around 
the environment and if the location of the sensor is known a grid based map can be built. 
Figure 8.2 shows the occupancy and empty maps for a map built with the CMU Terragator 
vehicle in figure 8.2. 

One problem with grid based methods is memory usage - it becomes expensive for 3D 
maps. However cunning data structures can overcome this. A second issue is that of grid 
alignment - it is arbitrary and as such cells are likely to cover areas that are awkwardly 
partially full as such is not possible to capture the sharp discontinuities at the edges of 
objects. Perhaps the biggest problem of grid based methods stems from the difficulties in 
localising with respect to a grid. It requires trying to match a small sensor centric map with 
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1. Plant Model 
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Figure 8.1: Graphical interpretation of a particle filter. The method is general but this 
diagram can be easily understood in terms of a range-to-beacon sensor (like the long baseline 
acoustic sensor mentioned in the Least Squares section). The best particles are those that 
best explain the actual range measurement. These are preferentially choosen to form the 
next generation. 
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Figure 8.2: From the early days of mobile robotics. The CMU terragator vehicle used grid 
based mapping in various settings. 




p l angle J , p (angle) 



Figure 8.3: The shape of two functions used to describe the probability P e (r) of empty cells 
at a range r from the sensor and the probability of them being occupied p {r). The actual 
object is at distance R from the sensor. The sensor modelled here is a wide beam sonar that 
sends out a cone of sound and measures the time between transmit and echo reception. 
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Figure 8.4: The two typical occupancy maps built by a grid based approach. The left map 
show the probability of regions being occupied and the left the probability of regions being 
empty. Each map is maintained using a different likelihood function 

a large global map. The vehicle can be localised by rotating and translating the local map 
(what is seen by the sensor) until the best match is found. 
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In Conclusion 



Hopefully you have found this introductory course interesting and have picked up a sense 
that the tools we have been using are applicable to many domains. If this were a 10 lecture 
course we would have spent more time considering how to appraise in real-time the validity 
of sensor data. This is a crucial point — we need to decide whether the data is even worth 
processing. In may situations sensors return grossly erroneous data (outliers). However if 
we don't know what the world looks like how can we be sure it is bad data and not faithful 
sensing of an unexpected world state? I also hope you have found the discussions on mobile 
robotics thought provoking. Perhaps in a decade we will have cars that avoid crashes, goods 
trucks that drive themselves, an always- deployed fleet of ocean vehicles monitoring El-Nino 
currents, smart machines on Mars and robots that massively increase the independence and 
mobility of our old and infirm. We don't need the "full-monty" of AI to be able to make a 
big difference.... (but it would be useful). 



P.M.N October 2003. 
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Miscellaneous Matters 



This section is meant to help you with the class work and covers things that you may need 
to know but that don't really fit in the main body of the notes 



10.1 Drawing Covariance Ellipses 



The n-dimensional multivariate-normal distribution has form 



P( X ) = ( 27r W2 I p |l/2 eX P^~2^ X ~ ^ TP X ( X ~^)} ( 1CL1 " 



The exponent (x — /X;c) T P _1 (x — jj x ) defines contours on the bell curve. A particularly 
useful contour is the "1 — a bound" where (x — /x^) P _1 (x — ji x ) = 1. It is really useful to 
be able to plot this contour to illustrate uncertainties in 2D. We can disregard the vector ji x 
as it simply shifts the distribution. So we have 

x T P _1 x = 1 (10.2) 

we can use eigenvalue decomposition to factorise P as VDV T where V is some rotation 
matrix and D is a diagonal matrix of principal values 

x T (VDV T )" 1 x = 1 (10.3) 
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p(x.y) 




Figure 10.1: A bi-variate normal distribution. The thick contour is the 1 — a bound where 
(x- t i x ) T P- 1 (x.-(t x ) = l 



using the fact that V is orthonormal: 



x r VD _1 V T x = 1 

x^VD-^D-^V^x = 1 
x T KK T x = 1 



(10.4) 
(10.5) 
(10.6) 



where 



K = VD 1/2 . 
now for any point y = [x,y] T which is on the unit circle, y T y = 1, so 

x T KK T x = y T y 



K T x 



VD 1 ^ 



(10.7) 



(10.8) 

(10.9) 

(10.10) 



So to draw an ellipse described by a 2 x 2 covariance matrix P we take a whole bunch of 
points on the unit circle and multiply them by VD 1 / 2 and plot the resulting points. This 
makes sense, intuitively the variance is in "units squared"-- D 1//2 is a diagonal matrix of 
standard deviations - the semi-major and semi-minor axes of the ellipse. The fist thing we 
do is scale the unit circle by these factors. Then we rotate the ellipse by V - - recall that 
the correlations between x and y (off diagonals in P ) induce a rotation in the ellipse? 
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Figure 10.2: The relationship between geometry of the 1 — a bound for a bivariate normal 
distribution and its covariance matrix. 

Figure 10.1 shows the relation ship between P and the plotted form for a bivariate normal 
distribution. 
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VII 
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10.2 Drawing High Dimensional Gaussians 



Obviously it is hard to draw a high dimensional gaussian on a screen or paper. For example 
we may have a covariance matrix that corresponds to a large state vector: 
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(10.13) 



(10.14) 



So to plot a 2D representation of the i th entity in a state vector simply plot the ellipse for 
the i th 2 by 2 block in P . 
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Example Code 



11.1 Matlab Code For Mars Lander Example 



70 

% A MARS LANDER EXAMPLE CODE C4 

% P. Newman 2003 

% 



% TOP LEVEL LOOP % 

function SimulateMarsLander 
close all ; clear all ; 

global Params;global XTrae;global VehicleStatus;global Store; 

%set up parameters 
Dolnitialise ; 

% initial conditions of estimator 

XEst = [ParamsXO+Params.InitialHeightError;Params.VO+Params.InitialVelocityError]; 

PEst = diag([Params.InitialHeightStd"2, Params. Initial VelocityStd" 2]); 

%store initial conditions: 
DoStore( 1 , XEst , PEst , [0] , [0] , NaN) ; 

k = 2; 

while( VehicleStatus. Landed & k <Params.StopTime/Params.dT) 

% simulate the world 
DoWorldSimulation(k) ; 
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% read from sensor 

z(k) = GetSensorData(k); 

% estimate the state of the vehicle 
[XEst,PEst,S,Innovation] = DoEstimation(XEst,PEst,z(k)); 

% make decisions based on our esimated state 
DoControl(k,XEst); 

% store results 

DoStore(k, XEst,PEst, Innovation, S,z(k)); 

%tick... 
k = k+l; 
end; 

%draw pictures.... 
DoMarsGraphics(k) ; 

return; 

% PROBLEM SET UP AND INITIALISATION 

% users changes parameters here 

function Dolnitialise 

global Params; 

global XTrue; 

global VehicleStatus; 

global Store; 

% user configurable parameters 

Params. StopTime = 600;%run for how many seconds (maximum)? 

Params. dT = 0.1; % Run navigation at 10Hz 

Params.cdight = 2.998e8; 

Params. EntryDrag =5; % linear drag constant 

Params. ChuteDrag = 2. 5*Params. EntryDrag; % linear drag constant with chute open 

Params. g = 9.8/3; % assumed gravity on mars 

Params. m = 50; % mass of vehcile 

Params. RocketBurnHeight = 1000; % when to turn on brakes 

Params. OpenChuteHeight = 4000; %when to open chute 

Params. X0 = 10000; % true entry height 

Params. V0 = 0; % true entry velocity 

Params. InitialHeightError = 0; % error on entry height 

Params. InitialVelocityError = 0; % error on entry velocity 

Params. InitialHeightStd = 100; %uncertainty in initial conditions 

Params. InitialVelocityStd = 20; %uncertainty in initial conditions 

Params. BurnTime = NaN; 

Params. ChuteTime = NaN; 

Params. LandingTime = NaN; 

% initial vehicle condition at entry into atmosphere... 
VehicleStatus. ChuteOpen = 0; 
VehicleStatus. RocketsOn = 0; 
VehicleStatus. Drag = Params. EntryDrag; 
VehicleStatus. Thrust = 0; 
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VehicleStatus. Landed = 0; 

%process plant model (constant velcoity with noise in acceleration ) 
Params.F = [1 Params.dT; 
1]; 

%process noise model (maps acceleration noise to other states ) 
Params.G = [Params.dT "2/2 ;Params.dT]; 

%actual process noise truely occuring — atmospher entry is a bumpy business 
%note this noise strength — not the deacceleration of the vehicle .... 
Params.SigmaQ = 0.2; %ms"{— 2} 

%process noise strength how much acceleration (varinace) in one tick 

% we expect (used to 'explain ' inaccuracies in our model) 

%the 3 is scale factor ( set it to 1 and real and modelled noises will 

%be equal 

Params.Q = (l.l*Params.SigmaQ)"2; %(ms"2 std) 

%observation model (explains observations in terms of state to be estimated) 
Params.H = [2/Params.cJight 0]; 

%observation noise strength (RTrue) is how noisey the sensor really is 
Params.SigmaR = 1.3e— 7; %(seconds) 3.0e— 7 corresponds to around 50m error. 

%observation expected noise strength (we never know this parameter exactly) 
%set the scale factor to 1 to make model and reallity match 
Params.R = (l.l*Params.SigmaR)"2; 

% initial conditions of (true) world: 
XTrue(:,l) = [Params.X0;Params.V0]; 

Params 
return; 

% MEASUREMENT SYSTEM 

function z = GetSensorData(k) 
global XTrue; 
global Params; 

z = Params. H*XTrue(:,k) + Params.SigmaR* randn(l); 
return; 



% ESTIMATION KALMAN FILTER 

function [XEst,PEst,S, Innovation] = DoEstimation(XEst,PEst,z) 

global Params; 

F = Params. F;G = Params. G;Q = Params. Q;R = Params. R;H = Params.H; 

%prediction... 

XPred = F*XEst; 

PPred = F*PEst*F'+G*Q*G'; 

% prepare for update... 
Innovation = z— H*XPred; 
S = H*PPred*H'+R; 
W = PPred*H'*inv(S); 
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% do update.... 

XEst = XPred+W*Innovation; 

PEst = PPred-W*S*W; 

return; 

% ITERATE SIMULATION = % 

function DoWorldSimulation(k) 

global XTrae;global Params;global VehicleStatus; 

oldvel = XTrue(2,k-l); 
oldpos = XTrue(l,k-l); 
dT = Params.dT; 

% friction is a function of height 

cxtau = 500; % spatial exponential factor for atmosphere density) 
AtmospherDensityScaleFactor = (1— exp( — (Params.XO— oldpos)/cxtau) ); 
c = AtmospherDensityScaleFactor*VehicleStatus.Drag; 

%clamp between and c for numerical safety 
c = min(max(c,0), VehicleStatus. Drag); 

%simple Euler integration 

ace = (— c*oldvel— Params.m*Params.g+ VehicleStatus. Thrust)/Params.m + Params.SigmaQ*randn(l); 

newvel = oldvel+acc*dT; 

newpos = oldpos+oldvel*dT+0.5*acc*dT"2; 

XTrue(:,k) = [newpos;newvel]; 



% LANDER CONTROL 

function DoControl(k,XEst) 
global Params;global VehicleStatus; 



if (XEst(l)<Params.OpenChuteHeight & "VehicleStatus. ChuteOpen) 

%open parachute: 

VehicleStatus. ChuteOpen = 1; 

VehicleStatus. Drag = Params.ChuteDrag; 

fprintf( 'Opening u Chute u at u time tJ %f\n',k*Params.dT); 

Params.ChuteTime = k*Params.dT; 
end; 

if (XEst(l)<Params.RocketBurnHeight ) 
if ("VehicleStatus. RocketsOn) 

fprintf( ' Releasing JI!hute^tJ;ime3of\n',k*Params.dT); 
fprintf('Firing^ockets^atJ;imeJ%f\n',k*Params.dT); 
Params.BurnTime = k*Params.dT; 
end; 

%turn on thrusters 
VehicleStatus. RocketsOn = 1; 
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%drop chute.. 
VehicleStatus.Drag = 0; 

%simple littel controller here (from v"2 = u"2+2as) and +mg for weight of vehicle 
VehicleStatus. Thrust = (Params.m*XEst(2)"2-l)/(2*XEst(l))+0.99*Params.m*Params.g; 

end; 

if(XEst(l)<l) 

%stop when we hit the ground... 

fprintf( 'Landed^at J;ime,_,%f\n',k*Params.dT); 

VehicleStatus. Landed = 1; 

Params.LandingTime = k*Params.dT; 

break; 
end; 

return; 

% MANAGE RESULTS STORAGE 

function DoStore(k,XEst,PEst, Innovation, S,z) 

global Store; 

if(k==l) 

Store. XEst = XEst; 

Store. PEst = diag(PEst); 

Store. Innovation = Innovation; 

Store. S = S; 

Store, z = z; 

else 

Store. XEst = [Store. XEst XEst]; 

Store. PEst = [Store. PEst diag(PEst)]; 

Store . Innovation = [ Store. Innovation Innovation]; 

Store. S = [Store. S diag(S)]; 

Store. z = [Store. z z]; 

end; 
return; 



11.2 Matlab Code For Ackerman Model Example 

function AckermannPredict 
clear all ; 
close all ; 

dT = 0.1;%time steps size 

nSteps = 600;%length of run 

L = 2;%length of vehicle 

SigmaV = 0.1; %3cm/s std on speed 

SigmaPhi = 4*pi/180; % steer inaccuracy 

% initial knowledge pdf (prior @ k = 0) 

P = diag ([0.2,0.2,0]); x = [0;0;0]; xtrue = x; 

Q = diag( [SigmaV "2 SigmaPhi "2]); 
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% Set up graphics % 

figure (1); hold on;axis equal;grid on;axis([— 20 20 —5 25]) 

xlabel('x');ylabel('y'); 

title ( ' uncertainty abounds Jbr.Ackermann jnodel'); 



% Main loop % 

for(k = l:nSteps) 

%control is a wiggle at constant velocity 

u = [l;pi/5*sin(4*pi*k/nSteps)]; 

%calculate jacobians 

JacFx =[10 -dT*u(l)*sin(x(3)); 1 dT*u(l)*cos(x(3)); 1]; 

JacPu = [dT*cos(x(3)) 0; dT*sin(x(3)) 0; dT* tan(u(2))/L dT *u(l)*sec(u(2))"2]; 

%prediction steps 

P = JacFx * P * JacFx' + JacFu*Q*JacFu'; 

xtrue = AckermannModel(xtrue,u+[SigmaV ;SigmaPhi].*randn(2,l),dT,L); 

x = AckermannModel(x,u,dT,L); 



%draw occasionally 
if(mod(k-l,30)==0) 

PlotEllipse(x,P,0.5); DrawRobot(x,'r');plot(xtrue(l),xtrue(2), 'ko' ); 
end; 
end; 

print — deps 'AckermannPredict.eps'%save the picture 



% MODEL 

function y = AckermannModel(x,u,dT,L) 
y(l,l) = x(l) + dT*u(l)*cos(x(3)); 
y(2,l) = x(2) + dT*u(l)*sin(x(3)); 
y(3,l) = x(3) + dT*u(l)/L*tan(u(2)); 

% Drawing Covariance % 

function eH = PlotEllipse(x,P,nSigma) 
P = P(l:2,l:2); % only plot x-y part 
x = x(l:2); 
if(~any(diag(P)==0)) 

[V,D] = eig(P); 

y = nSigma*[cos(0:0.1:2*pi);sin(0:0.1:2*pi )]; 

el = V*sqrtm(D)*y; 

el = [el el (:,l)]+repmat(x,l,size(el,2)+l); 

eH = line(el(l,:), el (2,:)); 
end; 



% Drawing Vehicle 

function DrawRobot(Xr,col); 



p=0.02; % percentage of axes size 
a=axis; 

ll=(a(2)-a(l))*p; 
12 = (a(4)-a(3))*p; 
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P=[-l 1 -1; -1 -1 3 -l];%basic triangle 

theta = Xr(3)— pi/2;%rotate to point along x axis (theta = 0) 

c=cos(theta); 

s=sin(theta); 

P=[c — s; s c]*P; %rotate by theta 

P(l,:)=P(l,:)*ll+Xr(l); %scale and shift to x 

P(2,:)=P(2,:)*12+Xr(2); 

H = plot(P(l,:),P(2,:),col,'LineWidth',0.1);% draw 

plot(Xr(l),Xr(2),sprintf('%s+',col)); 



11.3 Matlab Code For EKF Localisation Example 

function EKFLocalisation 

close all ; clear all ; 

global xTrue;global Map;global RTrue;global UTrue;global nSteps; 

nSteps = 6000; 

Map = 140*rand(2,30)-70; 

UTrue = diag([0.01,0.01,l*pi/180])."2; 
RTrue = diag([2.0,3*pi/180])."2; 

UEst = 1.0*UTrue; 
REst = 1.0*RTrue; 

xTrue = [l;-40;-pi/2]; 
xOdomLast = GetOdometry(l); 

% initial conditions : 

xEst =xTrue; 

PEst = diag([l,l,(l*pi/180)"2]); 



%%%%%%%%% storage %%%%%%%% 
InnovStore = NaN*zeros(2, nSteps); 
SStore = NaN*zeros(2,nSteps); 
PStore = NaN*zeros(3, nSteps); 
XStore = NaN*zeros(3, nSteps); 
XErrStore = NaN*zeros(3, nSteps); 

% initial graphics 

flgure(l); hold on; grid off; axis equal; 
plot(Map(l,:),Map(2,:),'g*');hold on; 
set(gcf, ' doublebuffer ' , 'on' ); 
hObsLine = line ([0,0], [0,0]); 
set(hObsLine,'linestyle ',':'); 

for k = 2:nSteps 

%do world iteration 
Simulate World(k) ; 

%figure out control 
xOdomNow = GetOdometry(k); 
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u = tcomp(tinv(xOdomLast),xOdomNow); 
xOdomLast = xOdomNow; 

%do prediction 

xPred = tcomp(xEst,u); 

xPred(3) = AngleWrap(xPred(3)); 

PPred = Jl(xEst,u)* PEst *Jl(xEst,u)' + J2(xEst,u)* UEst * J2(xEst,u)'; 

%observe a randomn feature 
[z,iFeature] = GetObservation(k); 

if (~isempty(z)) 

%predict observation 

zPred = DoObservationModel(xPred,iFeature,Map); 

% get observation Jacobian 

jH = GetObsJac(xPred,iFeature,Map); 

%do Kalman update: 

Innov = z— zPred; 

Innov(2) = AngleWrap(Innov(2)); 

S = jH*PPred*jH'+REst; 
W = PPred*jH'*inv(S); 
xEst = xPred+ Wrfnnov; 
xEst(3) = AngleWrap(xEst(3)); 

%note use of 'Joseph' form which is numerically stable 

I = eye(3); 

PEst = (I-W*jH)*PPred*(I-W*jH)'+ W*REst*W; 

PEst = 0.5*(PEst+PEst'); 

else 

%Ther was no observation available 
xEst = xPred; 
PEst = PPred; 
Innov = [NaN;NaN]; 
S = NaN*eye(2); 
end; 

if(mod(k-2,300)==0) 

DoVehicleGraphics(xEst,PEst (1:2,1:2),8,[0,1]); 
if(~isempty(z)) 

set(hObsLine,'XData',[xEst(l),Map(l,iFeature)]); 
set(hObsLine,'YData',[xEst(2),Map(2,iFeature)j); 
end; 

drawnow; 
end; 

%store results : 
InnovStore(:,k) = Innov; 
PStore(:,k) = sqrt(diag(PEst)); 
SStore(:,k) = sqrt(diag(S)); 
XStore(:,k) = xEst; 
XErrStore(:,k) = xTrue— xEst; 
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end; 

DoGraphs(InnovStore,PStore,SStore,XStore,XErrStore); 



70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 

function DoGraphs(InnovStore,PStore,SStore,XStore,XErrStore) 

figure (1); print — depsc 'EKFLocation.eps' 

figure (2); 

subplot(2,l,l);plot(InnovStore(l,:));hold on;plot(SStore(l,:),'r');plot(-SStore(l,:),'r') 

title ( 'Innovation' );ylabel( 'range' ); 

subplot(2,l,2);plot(InnovStore(2,:)*180/pi);hold on;plot(SStore(2,:)*180/pi,'r');plot(-SStore(2,:)*180/pi,'r') 

ylabel('Bearing tJ (deg)');xlabel('tinie'); 

print —depsc 'EKFLocationInnov.eps' 

figure (2); 

subplot(3,l,l);plot(XErrStore(l,:));holdon;plot(3*PStore(l,:),'r');plot(-3*PStore(l,:),'r'); 

title ( 'Covariance^and JCrror');ylabel('x'); 

subplot(3,l,2);plot(XErrStore(2,:));holdon;plot(3*PStore(2,:),'r');plot(-3*PStore(2,:),'r') 

ylabel('y'); 

subplot(3,l,3);plot(XErrStore(3,:)*180/pi);holdon;plot(3*PStore(3,:)*180/pi,'r');plot(-3*PStore(3,:)*180/pi,'r') 

ylabel('\theta');xlabel('time'); 

print —depsc 'EKFLocationErr.eps' 



70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 

function [z,iFeature] = GetObservation(k) 

global Map;global xTrue;global RTme;global nSteps; 

%fake sensor failure here 
if(abs(k-nSteps/2)<0.1*nSteps) 

z = D; 

iFeature = — 1; 
else 

iFeature = ceil(size(Map,2)*rand(l)); 

z = DoObservationModel(xTrue, iFeature, Map)+sqrt(RTrue)*randn(2,l); 

z(2) = AngleWrap(z(2)); 
end; 



70 70 70 70 70 70 70 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 

function [z] = DoObservationModel(xVeh, iFeature, Map) 
Delta = Map(l:2,iFeature)-xVeh(l:2); 
z = [norm (Delta); 

atan2(Delta(2),Delta(l))-xVeh(3)]; 
z(2) = AngleWrap(z(2)); 



70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 

function SimulateWorld(k) 
global xTrue; 
u = GetRobotControl(k); 
xTrue = tcomp(xTrue,u); 
xTrue(3) = AngleWrap(xTrue(3)); 



'o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7c 7o 7o 7o 7o 7o 7o 7o 7c 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 
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function jH = GetObsJac(xPred, iFeature,Map) 

jH = zeros(2,3); 

Delta = (Map(l:2,iFeature)-xPred(l:2)); 

r = norm(Delta); 

jH(l,l) = -Delta(l) / r; 

jH(l,2) = -Delta(2) / r; 

jH(2,l) = Delta(2)/(r"2); 

jH(2,2) = -Delta(l) / (r"2); 

jH(2,3) = -1; 



70 70 70 70 70 70 70 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 

function [xnow] = GetOdometry(k) 

persistent LastOdom; %internal to robot low— level controller 

global UTrue; 

if (isempty (LastOdom) ) 

global xTrue; 

LastOdom = xTrue; 
end; 

u = GetRobotControl(k); 
xnow =tcomp(LastOdom,u); 
uNoise = sqrt (UTrue) *randn(3,l); 
xnow = tcomp(xnow,uNoise); 
LastOdom = xnow; 



70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 

function u = GetRobotControl(k) 

global nSteps; 

u = [0; 0.025 ; 0.1* pi/180*sin(3*pi*k/nSteps)]; 

%u = [0; 0.15 ; 0.3* pi/180]; 



11.4 Matlab Code For EKF Mapping Example 

function EKFLocalisation 

close all ; clear all ; 

global xVehicleTrue;global Map;global RTrue;global UTrue;global nSteps; 



nSteps = 600; 

nFeatures = 6; 

MapSize = 200; 

Map = MapSize*rand(2, nFeatures)— MapSize/2; 

UTrue = diag([0.01,0.01,l*pi/180])."2; 
RTrue = diag([8.0,7*pi/180])."2; 

UEst = 1.0*UTrue; 
REst = 1.0*RTrue; 

xVehicleTrue = [l;-40;-pi/2]; 

% initial conditions — no map: 

xEst =[]; 

PEst = []; 

MappedFeatures = NaN*zeros(nFeatures,2); 
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%storage: 

PStore = NaN*zeros(nFeatures,nSteps); 

XErrStore = NaN*zeros(nFeatures,nSteps); 



% initial graphics — plot true map 
flgure(l); hold on; grid off; axis equal; 
plot(Map(l,:),Map(2,:),'g*');hold on; 
set(gcf, ' doublebuffer ' , 'on' ); 
hObsLine = line ([0,0], [0,0]); 
set(hObsLine,'linestyle ',':'); 



for k = 2:nSteps 

%do world iteration 
Simulate World(k) ; 

%simple prediction model: 
xPred = xEst; 
PPred = PEst; 

%observe a randomn feature 
[z,iFeature] = GetObservation(k); 

if ('isempty(z)) 

%have we seen this feature before? 

if ( ~ isnan(MappedFeatures(iFeature, 1) )) 

%predict observation: find out where it is in state vector 
Featurelndex = MappedFeatures(iFeature,l); 
xFeature = xPred(FeatureIndex:FeatureIndex+l); 
zPred = DoObservationModel(xVehicleTrue, xFeature); 

% get observation Jacobians 

[jHxvjHxf] = GetObsJacs(xVehicle True, xFeature); 

% fill in state jacobian 

jH = zeros(2,length(xEst)); 

jH (:,FeatureIndex:FeatureIndex+l) = jHxf; 

%do Kalman update: 

Innov = z— zPred; 

Innov(2) = AngleWrap(Innov(2)); 

S = jH*PPred*jH'+REst; 
W = PPred*jH'*inv(S); 
xEst = xPred+ W*Innov; 

PEst = PPred-W*S*W; 

%note use of 'Joseph' form which is numerically stable 
% I = eye(size(PEst)); 
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% PEst = (I-W*jH)*PPred*(I-W*jH)'+ W*REst*W; 

%ensure P remains symmetric 
PEst = 0.5*(PEst+PEst'); 
else 

% this is a new feature add it to the map.... 
nStates = length (xEst); 

xFeature = xVehicleTrue(l:2)+ [z(l)*cos(z(2)+xVehicleTrue(3));z(l)*sin(z(2)+xVehicleTrue(3))]; 

xEst = [xEst; xFeature]; 

[jGxv, jGz] = GetNewFeatureJacs(xVehicleTrue,z); 

M = [eye(nStates), zeros(nStates,2);% note we don't use jacobian w.r.t vehicle 
zeros(2,nStates) , jGz]; 

PEst = M*blkdiag(PEst,REst)*M'; 

%remember this feature as being mapped we store its ID and position in the state vector 
MappedFeatures(iFeature,:) = [length(xEst) — 1, length(xEst)]; 

end; 

else 

%There was no observation available 

end; 

if(mod(k-2,40)==0) 

plot(xVehicleTrue(l),xVehicleTrue(2),'r*'); 

%now draw all the estimated feature points 
DoMapGraphics(xEst,PEst,5) ; 

fprintf('k^=„%d\n',k); 

drawnow; 
end; 



%Storage: 

for(i = l:nFeatures) 

if (~isnan(MappedFeatures(i,l))) 
iL =MappedFeatures(i,l); 
PStore(k,i) = det(PEst(iL:iL+l,iL:iL+l)); 
XErrStore(k,i) = norm(xEst(iL:iL+l)-Map(:,i)); 
end; 
end; 



end; 



figure (2); 

plot(PStore); 
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70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 

function [z,iFeature] = GetObservation(k) 

global Map;global xVehicleTrue;global RTrue;global nSteps; 

%choose a random feature to see from True Map 

iFeature = ceil(size(Map,2)*rand(l)); 

z = DoObservationModel(xVehicleTrue ,Map ( : , iFeature) ) +sqrt (RTrue) *randn (2, 1 ) ; 

z(2) = AngleWrap(z(2)); 



70 70 70 70 70 70 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 

function [z] = DoObservationModel(xVeh, xFeature) 
Delta = xFeature-xVeh(l:2); 
z = [norm(Delta); 

atan2(Delta(2),Delta(l))-xVeh(3)]; 

z(2) = AngleWrap(z(2)); 



7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 

function SimulateWorld(k) 

global xVehicleTrue; 

u = GetRobotControl(k); 

xVehicleTrue = tcomp(xVehicleTrue,u); 



70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 

function [jHxvjHxf] = GetObsJacs(xPred, xFeature) 

jHxv = zeros(2,3);jHxf = zeros(2,2); 

Delta = (xFeature-xPred(l:2)); 

r = norm(Delta); 

jHxv(l,l) = -Delta(l) / r; 

jHxv(l,2) = -Delta(2) / r; 

jHxv(2,l) = Delta(2) / (r"2); 

jHxv(2,2) = -Delta(l) / (r"2); 

jHxv(2,3) = -1; 

jHxf(l:2,l:2) = -jHxv (1:2,1:2); 



70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 

function [jGxjGz] = GetNewFeatureJacs(Xv, z); 

x = Xv(l,l); 

y = Xv(2,l); 

theta = Xv(3,l); 

r = z(l); 

bearing = z(2); 

jGx = [ 1 — r*sin(theta + bearing); 

1 r*cos(theta + bearing)]; 
jGz = [ cos(theta + bearing) — r*sin(theta + bearing); 

sin(theta + bearing) r*cos(theta + bearing)]; 



70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 

function u = GetRobotControl(k) 

global nSteps; 

%u = [0; 0.25 ; 0.3*pi/180*sin(3*pi*k/nSteps)]; 

u = [0; 0.15 ; 0.3* pi/180]; 
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11.5 Matlab Code For EKF SLAM Example 

function EKFSLAM 

close all ; clear all ; 

global xVehicleTrue;global Map;global RTrue;global UTrue;global nSteps; 

global SensorSettings; 

%change these to alter sensor behaviour 
SensorSettings. FieldOfView = 45; 
SensorSettings. Range = 100; 

%how often shall we draw? 
DrawEveryNFrames = 50; 

%length of experiment 
nSteps = 8000; 



%when to take pictures? 

SnapShots = ceil(linspace(2, nSteps, 25)); 



%size of problem 

nFeatures = 40; 

MapSize = 200; 

Map = MapSize*rand(2, nFeatures)— MapSize/2; 

UTrue = diag([0.01,0.01,1.5*pi/180])."2; 
RTrue = diag([l.l,5*pi/180])."2; 

UEst = 2.0*UTrue; 
REst = 2.0*RTrue; 

xVehicleTrue = [0;0;-pi/2]; 

% initial conditions — no map: 

xEst = [xVehicleTrue] ; 

PEst = diag ([1,1,0.01]); 

MappedFeatures = NaN*zeros(nFeatures,2); 

%storage: 

PStore = NaN*zeros(nFeatures, nSteps); 

XErrStore = NaN*zeros(nFeatures, nSteps); 



% initial graphics — plot true map 
flgure(l); hold on; grid off; axis equal; 
plot(Map(l,:),Map(2,:),'g*');hold on; 
set(gcf, ' doublebuffer ' , 'on' ); 
hObsLine = line ([0,0], [0,0]); 
set(hObsLine,'linestyle ',':'); 
a = axis; axis(a*l.l); 



xOdomLast = GetOdometry(l); 
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for k = 2:nSteps 

%do world iteration 
SimulateWorld(k) ; 

%figure out control 

xOdomNow = GetOdometry(k); 

u = tcomp(tinv(xOdomLast),xOdomNow); 

xOdomLast = xOdomNow; 

%we'll need this lots ... 
xVehicle = xEst(l:3); 
xMap = xEst(4:end); 

%do prediction (the following is simply the result of multiplying 

%out block form of jacobians) 

xVehiclePred = tcomp (xVehicle, u); 

PPredvv = Jl(xVehicle,u)* PEst(l:3,l:3) *Jl(xVehicle,u)' + J2(xVehicle,u)* UEst * J2 (xVehicle, u)'; 

PPredvm = Jl(xVehicle,u)*PEst(l:3,4:end); 

PPredmm = PEst(4:end,4:end); 

xPred = [xVehiclePred;xMap]; 

PPred = [PPredvv PPredvm; 

PPredvm' PPredmm]; 

%observe a randomn feature 
[z,iFeature] = GetObservation(k); 

if (~isempty(z)) 

%have we seen this feature before? 

if ( ~ isnan(MappedFeatures(iFeature, 1) )) 

%predict observation: find out where it is in state vector 
Featurelndex = MappedFeatures(iFeature,l); 
xFeature = xPred(FeatureIndex:FeatureIndex+l); 

zPred = DoObservationModel(xVehicle, xFeature); 

% get observation Jacobians 

[jHxvjHxf] = GetObsJacs(xVehicle,xFeature); 

% fill in state jacobian 

jH = zeros(2,length(xEst)); 

jH (:,FeatureIndex:FeatureIndex+l) = jHxf; 

jH (:,1:3) = jHxv; 

%do Kalman update: 

Innov = z— zPred; 

Innov(2) = AngleWrap(Innov(2)); 

S = jH*PPred*jH'+REst; 
W = PPred*jH'*inv(S); 
xEst = xPred+ W*Innov; 
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PEst = PPred-W*S*W; 

%ensure P remains symmetric 
PEst = 0.5*(PEst+PEst'); 
else 

% this is a new feature add it to the map.... 
nStates = length (xEst); 

xFeature = xVehicle(l:2)+ [z(l)*cos(z(2)+xVehicle(3));z(l)*sin(z(2)+xVehicle(3))]; 
xEst = [xEst; xFeature]; %augmenting state vector 
[jGxv, jGz] = GetNewFeatureJacs(xVehicle,z); 

M = [eye(nStates), zeros(nStates,2);% note we don't use jacobian w.r.t vehicle 
jGxv zeros(2,nStates— 3) , jGz]; 

PEst = M*blkdiag(PEst,REst)*M'; 

%remember this feature as being mapped we store its ID and position in the state vector 
MappedFeatures(iFeature,:) = [length(xEst) — 1, length(xEst)]; 

end; 
else 

xEst = xPred; 
PESt = PPred; 
end; 



if (mod(k— 2,DrawEveryNFrames)==0) 
a = axis; 
elf; 

axis(a);hold on; 
n = length(xEst); 
nF = (n-3)/2; 
DoVehicleGraphics(xEst(l:3),PEst (1:3, 1:3), 3, [0 1]); 

if(~isnan(z)) 

h = HneQxEst (1), xFeature (l)],[xEst (2), xFeature (2)]); 

set(h,' linestyle ',':'); 
end; 
for(i = l:nF) 

iF = 3+2*i-l; 

plot(xEst(iF),xEst(iF+l),'b*'); 

PlotEllipse(xEst(iF:iF+l),PEst(iF:iF+l,iF:iF+l),3); 
end; 

fprintf('k„=„%d\n',k); 
drawnow; 
end; 

if (ismember(k, SnapShots)) 

iPic = flnd(SnapShots==k); 

print(gcf,'-depsc',sprintf('EKFSLAM%d.eps',iPic)); 
end; 



end; 
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7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 

function [z, iFeature] = GetObservation(k) 

global Map;global xVehicleTrue;global RTrue;global nSteps;global SensorSettings 

done = 0; 

Trys = 1; 

z = [] ; iFeature = — 1 ; 

while(~done & Trys <0.5*size(Map,2)) 

%choose a random feature to see from True Map 

iFeature = ceil(size(Map,2)*rand(l)); 

z = DoObservationModel(xVehicleTrue,Map(:, iFeature) )+sqrt(RTrue)*randn(2,l); 

z(2) = AngleWrap(z(2)); 

%look forward. ..and only up to 40m 

if (abs(pi/2— z(2))<SensorSettings.FieldOfView*pi/180 & z(l) < SensorSettings. Range) 

done =1 ; 
else 

Trys =Trys+l; 

z = [] ; iFeature = — 1 ; 
end; 
end; 



70 70 70 70 70 70 70 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 

function [z] = DoObservationModel(xVeh, xFeature) 
Delta = xFeature-xVeh(l:2); 
z = [norm(Delta); 

atan2(Delta(2),Delta(l))-xVeh(3)]; 

z(2) = AngleWrap(z(2)); 



70 70 70 70 70 70 70 7o 7o 7o 7o 70 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 

function SimulateWorld(k) 

global xVehicleTrue; 

u = GetRobotControl(k); 

xVehicleTrue = tcomp(xVehicleTrue,u); 



70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 

function [jHxvjHxf] = GetObsJacs(xPred, xFeature) 

jHxv = zeros(2,3);jHxf = zeros(2,2); 

Delta = (xFeature-xPred(l:2)); 

r = norm(Delta); 

jHxv(l,l) = -Delta(l) / r; 

jHxv(l,2) = -Delta(2) / r; 

jHxv(2,l) = Delta(2) / (r"2); 

jHxv(2,2) = -Delta(l) / (r"2); 

jHxv(2,3) = -1; 

jHxf(l:2,l:2) = -jHxv (1:2,1:2); 



70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 

function [jGxjGz] = GetNewFeatureJacs(Xv, z); 

x = Xv(l,l); 

y = Xv(2,l); 

theta = Xv(3,l); 

r = z(l); 

bearing = z(2); 

jGx = [ 1 — r*sin(theta + bearing); 
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1 r*cos(theta + bearing)]; 
jGz = [ cos(theta + bearing) — r*sin(theta + bearing); 
sin(theta + bearing) r*cos(theta + bearing)]; 



70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 

function [xnow] = GetOdometry(k) 

persistent LastOdom; %internal to robot low— level controller 

global UTrue; 

if (isempty (LastOdom) ) 

global xVehicleTrue; 

LastOdom = xVehicleTrue; 
end; 

u = GetRobotControl(k); 
xnow =tcomp(LastOdom,u); 
uNoise = sqrt (UTrue) *randn(3,l); 
xnow = tcomp(xnow,uNoise); 
LastOdom = xnow; 



70 70 70 70 70 70 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 

function u = GetRobotControl(k) 

global nSteps; 

%u = [0; 0.25 ; 0.3*pi/180*sin(3*pi*k/nSteps)]; 

u = [0; 0.15 ; 0.2* pi/180]; 

% Drawing Covariance % 

function eH = PlotEllipse(x,P,nSigma) 

eH = []; 

P = P(l:2,l:2); % only plot x-y part 

x = x(l:2); 

if(~any(diag(P)==0)) 

[V,D] = eig(P); 

y = nSigma*[cos(0:0.1:2*pi);sin(0:0.1:2*pi )]; 

el = V*sqrtm(D)*y; 

el = [el el (:,l)]+repmat(x,l,size(el,2)+l); 

eH = line(el (1,:), el (2,0 ); 
end; 



11.6 Matlab Code For Particle Filter Example 

%Monte— carlo based localisation 

%note this is not coded efficiently but rather to make the ideas clear 

%all loops should be vectorized but that gets a little matlab— speak intensive 

%and may obliterate the elegance of a particle filter .... 

function MCL 

close all ; clear all ; 

global xTrue;global Map;global RTrue;global UTrue;global nSteps; 



nSteps = 6000; 



?ichange this to see how sensitive we are to the number of particle 

dis 
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nParticles = 400; 

Map = 140*rand(2,30)-70; 

UTrue = diag([0.01,0.01,l*pi/180])/2; 
RTrue = diag([2.0,3*pi/180])."2; 

UEst = 1.0*UTrue; 
REst = 1.0*RTrue; 

xTrue = [l;-40;-pi/2]; 
xOdomLast = GetOdometry(l); 

% initial conditions: — a point cloud around truth 

xP =repmat(xTrue,l,nParticles)+diag( [8, 8,0. 4] )*randn(3, nParticles); 



%%%%%%%%% storage %%%%%%%% 

% initial graphics 

flgure(l); hold on; grid off; axis equal; 

plot(Map(l,:),Map(2,:),'g*');hold on; 

set(gcf, ' doublebuffer ' , 'on' ); 

hObsLine = line ([0,0], [0,0]); 

set(hObsLine,'linestyle ',':'); 

hPoints = plot(xP(l,:),xP(2,:), ' .' ); 

for k = 2:nSteps 

%do world iteration 
Simulate World(k) ; 



%all particles are equally important 
L = ones(nParticles,l)/ nParticles ; 



%figure out control 

xOdomNow = GetOdometry(k); 

u = tcomp(tinv(xOdomLast),xOdomNow); 

xOdomLast = xOdomNow; 

%do prediction 

%for each particle we add in control vector AND noise 
%the control noise adds diversity within the generation 
for(p = 1: nParticles) 

xP(:,p) = tcomp(xP(:,p),u+sqrt(UEst)*randn(3,l)); 
end; 



xP(3,:) = AngleWrap(xP(3,:)); 



%observe a randomn feature 
[z,iFeature] = GetObservation(k); 



if( isempty(z)) 
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%predict observation 



for(p = 1 : nParticles ) 

%what do we expect observation to be for this particle? 
zPred = DoObservationModel(xP(:,p),iFeature,Map); 



%how different 
Innov = z— zPred; 

%get likelihood (new importance). Assume gaussian here but any pdf works! 
%if predicted obs is very different from actual obs this score will be low 
%— >this particle is not very good at representing state . A lower score means 
%it is less likely to be selected for the next generation... 
L(p) = exp(— 0.5*Innov'*inv(REst)*Innov)+0.001; 



end; 
end; 



%reselect based on weights: 

%particles with big weights will occupy a greater percentage of the 

%y axis in a cummulative plot 

CDF = cumsum(L)/sum(L); 

%so randomly (uniform) choosing y values is more likely to correspond to 

%more likely (better) particles ... 

iSelect = rand(nParticles,l); 

%find the particle that corresponds to each y value (just a look up) 

iNextGeneration = interpl(CDF,l:nParticles,iSelect,'nearest','extrap' ); 

%copy selected particles for next generation .. 

xP = xP(:, iNextGeneration); 

%our estimate is simply the mean of teh particles 
xEst = mean(xP,2); 

if(mod(k-2,10)==0) 

figure (1); 

set(hPoints,'XData',xP(l,:)); 
set(hPoints,'YData',xP(2,:)); 
if (~isempty(z)) 

set(hObsLine,'XData',[xEst(l),Map(l,iFeature)]); 

set(hObsLine,'YData',[xEst(2),Map(2,iFeature)j); 
end; 

figure(2);plot(xP(l,:),xP (2, :),'.'); 
drawnow; 
end; 



end; 



70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 

function [z,iFeature] = GetObservation(k) 

global Map;global xTrue;global RTrue;global nSteps; 

%fake sensor failure here 
if(abs(k-nSteps/2)<0.1*nSteps) 
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z =U; 

iFeature = — 1; 
else 

iFeature = ceil(size(Map,2)*rand(l)); 

z = DoObservationModel(xTme, iFeature, Map)+sqrt(RTrue)*randn(2,l); 

z(2) = AngleWrap(z(2)); 
end; 



70 70 70 70 70 70 70 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 

function [z] = DoObservationModel(xVeh, iFeature, Map) 
Delta = Map(l:2,iFeature)-xVeh(l:2); 
z = [norm(Delta); 

atan2(Delta(2),Delta(l))-xVeh(3)]; 
z(2) = AngleWrap(z(2)); 



70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 

function SimulateWorld(k) 
global xTrue; 
u = GetRobotControl(k); 
xTrue = tcomp(xTrue,u); 
xTrue(3) = AngleWrap(xTrue(3)); 



70 70 70 70 70 70 70 7o 7o 7o 7o 70 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 7o 

function [xnow] = GetOdometry(k) 

persistent LastOdom; %internal to robot low— level controller 

global UTrue; 

if (isempty (LastOdom) ) 

global xTrue; 

LastOdom = xTrue; 
end; 

u = GetRobotControl(k); 
xnow =tcomp(LastOdom,u); 
uNoise = sqrt (UTrue) *randn(3,l); 
xnow = tcomp(xnow,uNoise); 
LastOdom = xnow; 



70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 70 

function u = GetRobotControl(k) 

global nSteps; 

u = [0; 0.025 ; 0.1* pi/180*sin(3*pi*k/nSteps)]; 
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